source: cpp/frams/util/3d.cpp

Last change on this file was 1022, checked in by Maciej Komosinski, 4 years ago

Cosmetic

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