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

Last change on this file since 303 was 303, checked in by Maciej Komosinski, 9 years ago

Improved error messages

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