source: cpp/frams/util/3d.h @ 305

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

Increased precision / less optimizations in some rotations and computation of angles

  • Property svn:eol-style set to native
File size: 6.0 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#ifndef _3D_H_
6#define _3D_H_
7
8#if defined SHP || defined __ANDROID__
9#include <string.h> //memcpy
10#else
11#include <memory.h> //memcpy
12#endif
13
14/**********************************
15\file 3d.h 3d.cpp
16
17basic 3D classes and operators
18*********************************/
19
20/// point in 3D space
21class Pt3D
22{
23public:
24        double x, y, z;
25        static bool report_errors;
26
27        Pt3D(double _x, double _y, double _z) :x(_x), y(_y), z(_z) {} ///< constructor initializing all coords
28        Pt3D(double xyz) :x(xyz), y(xyz), z(xyz) {} ///< all coords equal
29        Pt3D() {} ///< coords will be not initialized!
30        Pt3D(const Pt3D &p) :x(p.x), y(p.y), z(p.z) {} ///< copy from another point
31        bool    operator==(const Pt3D& p)       { return (x == p.x) && (y == p.y) && (z == p.z); }
32        void    operator+=(const Pt3D& p)       { x += p.x; y += p.y; z += p.z; }
33        void    operator-=(const Pt3D& p)       { x -= p.x; y -= p.y; z -= p.z; }
34        void    operator*=(double d)    { x *= d; y *= d; z *= d; }
35        Pt3D    operator*(const Pt3D &p) const { return Pt3D(y*p.z - z*p.y, z*p.x - x*p.z, x*p.y - y*p.x); }
36        void    operator/=(double d)    { x /= d; y /= d; z /= d; }
37        //Pt3D  operator+(const Pt3D& p) const {return Pt3D(x+p.x,y+p.y,z+p.z);}
38        //Pt3D  operator-(const Pt3D& p) const {return Pt3D(x-p.x,y-p.y,z-p.z);}
39        Pt3D    operator-() const { return Pt3D(-x, -y, -z); }
40        Pt3D    operator*(double d) const { return Pt3D(x*d, y*d, z*d); }
41        Pt3D    operator/(double d) const { return Pt3D(x / d, y / d, z / d); }
42        bool    allCoordsLowerThan(const Pt3D& p) const { return (x < p.x) && (y < p.y) && (z<p.z); }
43        bool    allCoordsHigherThan(const Pt3D& p) const { return (x>p.x) && (y > p.y) && (z > p.z); }
44        void getMin(const Pt3D& p);
45        void getMax(const Pt3D& p);
46        /** vector length = \f$\sqrt{x^2+y^2+z^2}\f$  */
47        double operator()() const;
48        /** vector length = \f$\sqrt{x^2+y^2+z^2}\f$  */
49        double length() const { return operator()(); }
50        double length2() const { return x*x + y*y + z*z; }
51        double distanceTo(const Pt3D& p) const;
52        double manhattanDistanceTo(const Pt3D& p) const;
53        /** calculate angle between (0,0)-(dx,dy), @return 1=ok, 0=can't calculate */
54        static double getAngle(double dx, double dy);
55        /** calculate 3 rotation angles translating (1,0,0) into 'X' and (0,0,1) into 'dir' */
56        void getAngles(const Pt3D& X, const Pt3D& dir);
57        void vectorProduct(const Pt3D& a, const Pt3D& b);
58        Pt3D vectorProduct(const Pt3D& p) const { return (*this)*p; }
59        Pt3D entrywiseProduct(const Pt3D &p) const { return Pt3D(x*p.x, y*p.y, z*p.z); } ///< also known as Hadamard product or Schur product
60        double dotProduct(const Pt3D& p) const { return x*p.x + y*p.y + z*p.z; }
61        bool normalize();
62};
63
64Pt3D operator+(const Pt3D &p1, const Pt3D &p2);
65Pt3D operator-(const Pt3D &p1, const Pt3D &p2);
66
67class Pt3D_DontReportErrors
68{
69        bool state;
70public:
71        Pt3D_DontReportErrors() { state = Pt3D::report_errors; Pt3D::report_errors = false; }
72        ~Pt3D_DontReportErrors() { Pt3D::report_errors = state; }
73};
74
75///  orientation in 3D space = rotation matrix
76class Matrix44;
77
78class Orient
79{
80public:
81        Pt3D x, y, z; ///< 3 vectors (= 3x3 matrix)
82
83        Orient() {}
84        Orient(const Orient& src) { x = src.x; y = src.y; z = src.z; }
85        Orient(const Pt3D& a, const Pt3D& b, const Pt3D& c) :x(a), y(b), z(c) {}
86        //      Orient(const Pt3D& rot) {*this=rot;}
87        Orient(const Matrix44& m);
88        void operator=(const Pt3D &rot);
89        void rotate(const Pt3D &); ///< rotate matrix around 3 axes
90
91        void transform(Pt3D &target, const Pt3D &src) const;    ///< transform a vector
92        void revTransform(Pt3D &target, const Pt3D &src) const; ///< reverse transform
93        Pt3D transform(const Pt3D &src) const { Pt3D t; transform(t, src); return t; }
94        Pt3D revTransform(const Pt3D &src) const { Pt3D t; revTransform(t, src); return t; }
95
96        void transform(Orient& target, const Orient& src) const;    ///< transform other orient
97        void revTransform(Orient& target, const Orient& src) const; ///< reverse transform other orient
98        Orient transform(const Orient& src) const { Orient o; transform(o, src); return o; }    ///< transform other orient
99        Orient revTransform(const Orient& src) const { Orient o; revTransform(o, src); return o; } ///< reverse transform other orient
100
101        void transformSelf(const Orient &rot) { Orient tmp; rot.transform(tmp, *this); *this = tmp; }
102        void revTransformSelf(const Orient &rot) { Orient tmp; rot.revTransform(tmp, *this); *this = tmp; }
103
104        void getAngles(Pt3D &) const; ///< calculate rotation from current matrix
105        Pt3D getAngles() const { Pt3D ret; getAngles(ret); return ret; }; ///< calculate rotation from current matrix
106        void lookAt(const Pt3D &X, const Pt3D &dir); ///< calculate orientation matrix from 2 vectors: X becomes (normalized) Orient.x, dir is the preferred "up" direction (Orient.z). Use lookAt(Pt3D) if only X is relevant.
107        void lookAt(const Pt3D &X); ///< calculate orientation matrix from 1 vector, X becomes (normalized) Orient.x, the other coordinates are deterministic but not continuous. Use lookAt(Pt3D,Pt3D) if you need more control.
108
109        bool normalize();
110};
111
112class Matrix44
113{
114public:
115        double m[16];
116        Matrix44() {}
117        Matrix44(const Matrix44& src) { memcpy(m, src.m, sizeof(m)); }
118        Matrix44(double *srcm) { memcpy(m, srcm, sizeof(m)); }
119        Matrix44(const Orient &rot);
120
121        const double& operator()(int i, int j) const { return m[i + 16 * j]; }
122        const double& operator[](int i) const { return m[i]; }
123        double& operator()(int i, int j) { return m[i + 16 * j]; }
124        double& operator[](int i) { return m[i]; }
125
126        void operator+=(const Pt3D &); ///< translate matrix
127        void operator*=(const Pt3D &); ///< scale matrix
128        void operator*=(double sc); ///< scale matrix
129};
130
131extern Pt3D Pt3D_0; ///< zero vector
132extern Orient Orient_1; ///< standard unit matrix: 100 010 001
133extern Matrix44 Matrix44_1; ///< standard unit matrix: 1000 0100 0010 0001
134
135void rotate2D(double, double &, double &); ///< rotate 2d vector, given angle
136void rotate2D(double, double, double &, double &); ///< rotate 2d vector, given sin and cos
137double d2(double, double); ///< distance in 2D
138
139#endif
Note: See TracBrowser for help on using the repository browser.