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

Last change on this file since 375 was 375, checked in by Maciej Komosinski, 7 years ago

Renamed logging functions to more intuitive and simple names

  • Property svn:eol-style set to native
File size: 5.5 KB
Line 
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/log.h>
7#include "3d.h"
8
9Pt3D operator+(const Pt3D &p1, const Pt3D &p2) { return Pt3D(p1.x + p2.x, p1.y + p2.y, p1.z + p2.z); }
10Pt3D operator-(const Pt3D &p1, const Pt3D &p2) { return Pt3D(p1.x - p2.x, p1.y - p2.y, p1.z - p2.z); }
11
12Pt3D Pt3D_0(0, 0, 0);
13
14bool Pt3D::report_errors = true;
15
16double Pt3D::operator()() const
17{
18        double q = x*x + y*y + z*z;
19        if (q < 0) { if (report_errors) logPrintf("Pt3D", "operator()", LOG_ERROR, "sqrt(%g): domain error", q); return 0; }
20        return sqrt(q);
21}
22
23bool Pt3D::normalize()
24{
25        double len = length();
26        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; }
27        operator/=(len);
28        return true;
29}
30
31double 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
39double Pt3D::manhattanDistanceTo(const Pt3D& p) const
40{
41        return fabs(x - p.x) + fabs(y - p.y) + fabs(z - p.z);
42}
43
44Orient Orient_1(Pt3D(1, 0, 0), Pt3D(0, 1, 0), Pt3D(0, 0, 1));
45
46// simple rotation
47void 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
55void 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
62double Pt3D::getAngle(double dx, double dy)
63{
64        if (dx == 0 && dy == 0)
65        {
66                if (report_errors) logPrintf("Pt3D", "getAngle()", LOG_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
72void 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
94void 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}
100void 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
107void 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
114void 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
123void 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
146double d2(double x, double y)
147{
148        double q = x*x + y*y;
149        if (q < 0) { if (Pt3D::report_errors) logPrintf("", "d2()", LOG_ERROR, "sqrt(%g): domain error", q); return 0; }
150        return sqrt(q);
151}
152
153Orient::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
160void Orient::operator=(const Pt3D &rot)
161{
162        *this = Orient_1;
163        rotate(rot);
164}
165
166void 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
192void 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
199void 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
206void 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
213void 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
220void Orient::getAngles(Pt3D &angles) const
221{
222        angles.getAngles(x, z);
223}
224
225bool 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
236Matrix44::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
244void Matrix44::operator+=(const Pt3D &)
245{
246
247}
248
249void Matrix44::operator*=(const Pt3D &)
250{
251}
252
253void Matrix44::operator*=(double sc)
254{
255}
Note: See TracBrowser for help on using the repository browser.