1 | // This file is a part of Framsticks SDK. http://www.framsticks.com/ |
---|
2 | // Copyright (C) 1999-2015 Maciej Komosinski and Szymon Ulatowski. |
---|
3 | // See LICENSE.txt for details. |
---|
4 | |
---|
5 | #include <common/nonstd_math.h> |
---|
6 | #include <common/framsg.h> |
---|
7 | #include "3d.h" |
---|
8 | |
---|
9 | Pt3D operator+(const Pt3D &p1, const Pt3D &p2) { return Pt3D(p1.x + p2.x, p1.y + p2.y, p1.z + p2.z); } |
---|
10 | Pt3D operator-(const Pt3D &p1, const Pt3D &p2) { return Pt3D(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z); } |
---|
11 | |
---|
12 | Pt3D Pt3D_0(0, 0, 0); |
---|
13 | |
---|
14 | bool Pt3D::report_errors = true; |
---|
15 | |
---|
16 | double Pt3D::operator()() const |
---|
17 | { |
---|
18 | double q = x*x + y*y + z*z; |
---|
19 | if (q < 0) { if (report_errors) FMprintf("Pt3D", "operator()", FMLV_ERROR, "sqrt(%g): domain error", q); return 0; } |
---|
20 | return sqrt(q); |
---|
21 | } |
---|
22 | |
---|
23 | bool Pt3D::normalize() |
---|
24 | { |
---|
25 | double len = length(); |
---|
26 | if (fabs(len) < 1e-50) { if (report_errors) FMprintf("Pt3D", "normalize()", FMLV_WARN, "vector[%g,%g,%g] too small", x, y, z); x = 1; y = 0; z = 0; return false; } |
---|
27 | operator/=(len); |
---|
28 | return true; |
---|
29 | } |
---|
30 | |
---|
31 | double Pt3D::distanceTo(const Pt3D& p) const |
---|
32 | { |
---|
33 | double dx = x - p.x; |
---|
34 | double dy = y - p.y; |
---|
35 | double dz = z - p.z; |
---|
36 | return sqrt(dx*dx + dy*dy + dz*dz); |
---|
37 | } |
---|
38 | |
---|
39 | double Pt3D::manhattanDistanceTo(const Pt3D& p) const |
---|
40 | { |
---|
41 | return fabs(x - p.x) + fabs(y - p.y) + fabs(z - p.z); |
---|
42 | } |
---|
43 | |
---|
44 | Orient Orient_1(Pt3D(1, 0, 0), Pt3D(0, 1, 0), Pt3D(0, 0, 1)); |
---|
45 | |
---|
46 | // simple rotation |
---|
47 | void rotate2D(double k, double &x, double &y) |
---|
48 | { |
---|
49 | double s = sin(k), c = cos(k); |
---|
50 | double t = c*x - s*y; |
---|
51 | y = s*x + c*y; |
---|
52 | x = t; |
---|
53 | } |
---|
54 | |
---|
55 | void rotate2D(double s, double c, double &x, double &y) |
---|
56 | { |
---|
57 | double t = c*x - s*y; |
---|
58 | y = s*x + c*y; |
---|
59 | x = t; |
---|
60 | } |
---|
61 | |
---|
62 | double Pt3D::getAngle(double dx, double dy) |
---|
63 | { |
---|
64 | if (dx == 0 && dy == 0) |
---|
65 | { |
---|
66 | if (report_errors) FMprintf("Pt3D", "getAngle()", FMLV_WARN, "atan2(%g,%g)", dy, dx); |
---|
67 | return 0; // incorrect result, but there is no correct one |
---|
68 | } |
---|
69 | return atan2(dy, dx); |
---|
70 | } |
---|
71 | |
---|
72 | void Pt3D::getAngles(const Pt3D& X, const Pt3D& dir) |
---|
73 | { |
---|
74 | Pt3D t1(X), t2(dir); |
---|
75 | if (fabs(t1.x) > 1e-50 || fabs(t1.y) > 1e-50) // non-vertical |
---|
76 | { |
---|
77 | z = atan2(t1.y, t1.x); |
---|
78 | rotate2D(-z, t1.x, t1.y); |
---|
79 | rotate2D(-z, t2.x, t2.y); |
---|
80 | y = getAngle(t1.x, t1.z); |
---|
81 | } |
---|
82 | else // vertical |
---|
83 | { |
---|
84 | z = 0; |
---|
85 | if (t1.z < 0) |
---|
86 | y = -M_PI_2; // down |
---|
87 | else |
---|
88 | y = M_PI_2; // up |
---|
89 | } |
---|
90 | rotate2D(-y, t2.x, t2.z); |
---|
91 | x = getAngle(t2.z, -t2.y); |
---|
92 | } |
---|
93 | |
---|
94 | void Pt3D::getMin(const Pt3D& p) |
---|
95 | { |
---|
96 | if (p.x < x) x = p.x; |
---|
97 | if (p.y < y) y = p.y; |
---|
98 | if (p.z < z) z = p.z; |
---|
99 | } |
---|
100 | void Pt3D::getMax(const Pt3D& p) |
---|
101 | { |
---|
102 | if (p.x > x) x = p.x; |
---|
103 | if (p.y > y) y = p.y; |
---|
104 | if (p.z > z) z = p.z; |
---|
105 | } |
---|
106 | |
---|
107 | void Pt3D::vectorProduct(const Pt3D& a, const Pt3D& b) |
---|
108 | { |
---|
109 | x = a.y*b.z - a.z*b.y; |
---|
110 | y = a.z*b.x - a.x*b.z; |
---|
111 | z = a.x*b.y - a.y*b.x; |
---|
112 | } |
---|
113 | |
---|
114 | void Orient::lookAt(const Pt3D& X, const Pt3D& dir) |
---|
115 | { |
---|
116 | x = X; x.normalize(); |
---|
117 | y.vectorProduct(dir, x); |
---|
118 | z.vectorProduct(x, y); |
---|
119 | if ((!y.normalize()) || (!z.normalize())) |
---|
120 | lookAt(X);// dir was (nearly?) parallel, there is no good solution, use the x-only variant |
---|
121 | } |
---|
122 | |
---|
123 | void Orient::lookAt(const Pt3D& X) |
---|
124 | { |
---|
125 | x = X; x.normalize(); |
---|
126 | // "invent" y vector, not parallel to x |
---|
127 | double ax = fabs(x.x), ay = fabs(x.y), az = fabs(x.z); |
---|
128 | // find the smallest component |
---|
129 | if ((ax <= ay) && (ax <= az)) // x |
---|
130 | { |
---|
131 | y.x = 0; y.y = -x.z; y.z = x.y; // (0,-z,y) |
---|
132 | } |
---|
133 | if ((ay <= ax) && (ay <= az)) // y |
---|
134 | { |
---|
135 | y.x = -x.z; y.y = 0; y.z = x.x; // (-z,0,x) |
---|
136 | } |
---|
137 | else // z |
---|
138 | { |
---|
139 | y.x = -x.y; y.y = x.x; y.z = 0; // (-y,x,0) |
---|
140 | } |
---|
141 | y.normalize(); |
---|
142 | z.vectorProduct(x, y); |
---|
143 | } |
---|
144 | |
---|
145 | // 2D distance |
---|
146 | double d2(double x, double y) |
---|
147 | { |
---|
148 | double q = x*x + y*y; |
---|
149 | if (q < 0) { if (Pt3D::report_errors) FMprintf("", "d2()", FMLV_ERROR, "sqrt(%g): domain error", q); return 0; } |
---|
150 | return sqrt(q); |
---|
151 | } |
---|
152 | |
---|
153 | Orient::Orient(const Matrix44& m) |
---|
154 | { |
---|
155 | x.x = m[0]; x.y = m[1]; x.z = m[2]; |
---|
156 | y.x = m[4]; y.y = m[5]; y.z = m[6]; |
---|
157 | z.x = m[8]; z.y = m[9]; z.z = m[10]; |
---|
158 | } |
---|
159 | |
---|
160 | void Orient::operator=(const Pt3D &rot) |
---|
161 | { |
---|
162 | *this = Orient_1; |
---|
163 | rotate(rot); |
---|
164 | } |
---|
165 | |
---|
166 | void Orient::rotate(const Pt3D &v) |
---|
167 | { |
---|
168 | double s, c; |
---|
169 | if (v.x != 0) |
---|
170 | { |
---|
171 | s = sin(v.x); c = cos(v.x); |
---|
172 | rotate2D(s, c, x.y, x.z); |
---|
173 | rotate2D(s, c, y.y, y.z); |
---|
174 | rotate2D(s, c, z.y, z.z); |
---|
175 | } |
---|
176 | if (v.y != 0) |
---|
177 | { |
---|
178 | s = sin(v.y); c = cos(v.y); |
---|
179 | rotate2D(s, c, x.x, x.z); |
---|
180 | rotate2D(s, c, y.x, y.z); |
---|
181 | rotate2D(s, c, z.x, z.z); |
---|
182 | } |
---|
183 | if (v.z != 0) |
---|
184 | { |
---|
185 | s = sin(v.z); c = cos(v.z); |
---|
186 | rotate2D(s, c, x.x, x.y); |
---|
187 | rotate2D(s, c, y.x, y.y); |
---|
188 | rotate2D(s, c, z.x, z.y); |
---|
189 | } |
---|
190 | } |
---|
191 | |
---|
192 | void Orient::transform(Pt3D& target, const Pt3D &s) const |
---|
193 | { |
---|
194 | target.x = s.x*x.x + s.y*y.x + s.z*z.x; |
---|
195 | target.y = s.x*x.y + s.y*y.y + s.z*z.y; |
---|
196 | target.z = s.x*x.z + s.y*y.z + s.z*z.z; |
---|
197 | } |
---|
198 | |
---|
199 | void Orient::revTransform(Pt3D& target, const Pt3D &s) const |
---|
200 | { |
---|
201 | target.x = s.x*x.x + s.y*x.y + s.z*x.z; |
---|
202 | target.y = s.x*y.x + s.y*y.y + s.z*y.z; |
---|
203 | target.z = s.x*z.x + s.y*z.y + s.z*z.z; |
---|
204 | } |
---|
205 | |
---|
206 | void Orient::transform(Orient& target, const Orient& src) const |
---|
207 | { |
---|
208 | transform(target.x, src.x); |
---|
209 | transform(target.y, src.y); |
---|
210 | transform(target.z, src.z); |
---|
211 | } |
---|
212 | |
---|
213 | void Orient::revTransform(Orient& target, const Orient& src) const |
---|
214 | { |
---|
215 | revTransform(target.x, src.x); |
---|
216 | revTransform(target.y, src.y); |
---|
217 | revTransform(target.z, src.z); |
---|
218 | } |
---|
219 | |
---|
220 | void Orient::getAngles(Pt3D &angles) const |
---|
221 | { |
---|
222 | angles.getAngles(x, z); |
---|
223 | } |
---|
224 | |
---|
225 | bool Orient::normalize() |
---|
226 | { |
---|
227 | bool ret = 1; |
---|
228 | y.vectorProduct(z, x); |
---|
229 | z.vectorProduct(x, y); |
---|
230 | if (!x.normalize()) ret = 0; |
---|
231 | if (!z.normalize()) ret = 0; |
---|
232 | if (!y.normalize()) ret = 0; |
---|
233 | return ret; |
---|
234 | } |
---|
235 | |
---|
236 | Matrix44::Matrix44(const Orient &rot) |
---|
237 | { |
---|
238 | m[0] = rot.x.x; m[1] = rot.x.y; m[2] = rot.x.z; m[3] = 0; |
---|
239 | m[4] = rot.y.x; m[5] = rot.y.y; m[6] = rot.y.z; m[7] = 0; |
---|
240 | m[8] = rot.z.x; m[9] = rot.z.y; m[10] = rot.z.z; m[11] = 0; |
---|
241 | m[12] = 0; m[13] = 0; m[14] = 0; m[15] = 1; |
---|
242 | } |
---|
243 | |
---|
244 | void Matrix44::operator+=(const Pt3D &) |
---|
245 | { |
---|
246 | |
---|
247 | } |
---|
248 | |
---|
249 | void Matrix44::operator*=(const Pt3D &) |
---|
250 | { |
---|
251 | } |
---|
252 | |
---|
253 | void Matrix44::operator*=(double sc) |
---|
254 | { |
---|
255 | } |
---|