[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> |
---|
[375] | 6 | #include <common/log.h> |
---|
[109] | 7 | #include "3d.h" |
---|
| 8 | |
---|
[255] | 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); } |
---|
[109] | 11 | |
---|
[255] | 12 | Pt3D Pt3D_0(0, 0, 0); |
---|
[109] | 13 | |
---|
[255] | 14 | bool Pt3D::report_errors = true; |
---|
[109] | 15 | |
---|
| 16 | double Pt3D::operator()() const |
---|
| 17 | { |
---|
[255] | 18 | double q = x*x + y*y + z*z; |
---|
[375] | 19 | if (q < 0) { if (report_errors) logPrintf("Pt3D", "operator()", LOG_ERROR, "sqrt(%g): domain error", q); return 0; } |
---|
[255] | 20 | return sqrt(q); |
---|
[109] | 21 | } |
---|
| 22 | |
---|
| 23 | bool Pt3D::normalize() |
---|
| 24 | { |
---|
[255] | 25 | double len = length(); |
---|
[375] | 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; } |
---|
[255] | 27 | operator/=(len); |
---|
| 28 | return true; |
---|
[109] | 29 | } |
---|
| 30 | |
---|
| 31 | double Pt3D::distanceTo(const Pt3D& p) const |
---|
| 32 | { |
---|
[305] | 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); |
---|
[109] | 37 | } |
---|
| 38 | |
---|
| 39 | double Pt3D::manhattanDistanceTo(const Pt3D& p) const |
---|
| 40 | { |
---|
[255] | 41 | return fabs(x - p.x) + fabs(y - p.y) + fabs(z - p.z); |
---|
[109] | 42 | } |
---|
| 43 | |
---|
[255] | 44 | Orient Orient_1(Pt3D(1, 0, 0), Pt3D(0, 1, 0), Pt3D(0, 0, 1)); |
---|
[109] | 45 | |
---|
[303] | 46 | // simple rotation |
---|
[255] | 47 | void rotate2D(double k, double &x, double &y) |
---|
| 48 | { |
---|
| 49 | double s = sin(k), c = cos(k); |
---|
[305] | 50 | double t = c*x - s*y; |
---|
| 51 | y = s*x + c*y; |
---|
| 52 | x = t; |
---|
[255] | 53 | } |
---|
[109] | 54 | |
---|
[255] | 55 | void rotate2D(double s, double c, double &x, double &y) |
---|
| 56 | { |
---|
[305] | 57 | double t = c*x - s*y; |
---|
| 58 | y = s*x + c*y; |
---|
| 59 | x = t; |
---|
[255] | 60 | } |
---|
[109] | 61 | |
---|
[305] | 62 | double Pt3D::getAngle(double dx, double dy) |
---|
[109] | 63 | { |
---|
[305] | 64 | if (dx == 0 && dy == 0) |
---|
| 65 | { |
---|
[375] | 66 | if (report_errors) logPrintf("Pt3D", "getAngle()", LOG_WARN, "atan2(%g,%g)", dy, dx); |
---|
[305] | 67 | return 0; // incorrect result, but there is no correct one |
---|
| 68 | } |
---|
| 69 | return atan2(dy, dx); |
---|
[109] | 70 | } |
---|
| 71 | |
---|
[255] | 72 | void Pt3D::getAngles(const Pt3D& X, const Pt3D& dir) |
---|
[109] | 73 | { |
---|
[305] | 74 | Pt3D t1(X), t2(dir); |
---|
| 75 | if (fabs(t1.x) > 1e-50 || fabs(t1.y) > 1e-50) // non-vertical |
---|
[303] | 76 | { |
---|
[321] | 77 | z = atan2(t1.y, t1.x); |
---|
[255] | 78 | rotate2D(-z, t1.x, t1.y); |
---|
| 79 | rotate2D(-z, t2.x, t2.y); |
---|
[305] | 80 | y = getAngle(t1.x, t1.z); |
---|
[109] | 81 | } |
---|
[255] | 82 | else // vertical |
---|
[303] | 83 | { |
---|
[255] | 84 | z = 0; |
---|
| 85 | if (t1.z < 0) |
---|
| 86 | y = -M_PI_2; // down |
---|
| 87 | else |
---|
| 88 | y = M_PI_2; // up |
---|
[109] | 89 | } |
---|
[255] | 90 | rotate2D(-y, t2.x, t2.z); |
---|
[305] | 91 | x = getAngle(t2.z, -t2.y); |
---|
[109] | 92 | } |
---|
| 93 | |
---|
| 94 | void Pt3D::getMin(const Pt3D& p) |
---|
| 95 | { |
---|
[255] | 96 | if (p.x < x) x = p.x; |
---|
| 97 | if (p.y < y) y = p.y; |
---|
| 98 | if (p.z < z) z = p.z; |
---|
[109] | 99 | } |
---|
| 100 | void Pt3D::getMax(const Pt3D& p) |
---|
| 101 | { |
---|
[255] | 102 | if (p.x > x) x = p.x; |
---|
| 103 | if (p.y > y) y = p.y; |
---|
| 104 | if (p.z > z) z = p.z; |
---|
[109] | 105 | } |
---|
| 106 | |
---|
[255] | 107 | void Pt3D::vectorProduct(const Pt3D& a, const Pt3D& b) |
---|
[109] | 108 | { |
---|
[255] | 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; |
---|
[109] | 112 | } |
---|
| 113 | |
---|
[255] | 114 | void Orient::lookAt(const Pt3D& X, const Pt3D& dir) |
---|
[109] | 115 | { |
---|
[255] | 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 |
---|
[109] | 130 | { |
---|
[255] | 131 | y.x = 0; y.y = -x.z; y.z = x.y; // (0,-z,y) |
---|
[303] | 132 | } |
---|
[255] | 133 | if ((ay <= ax) && (ay <= az)) // y |
---|
| 134 | { |
---|
| 135 | y.x = -x.z; y.y = 0; y.z = x.x; // (-z,0,x) |
---|
[303] | 136 | } |
---|
[255] | 137 | else // z |
---|
| 138 | { |
---|
| 139 | y.x = -x.y; y.y = x.x; y.z = 0; // (-y,x,0) |
---|
[303] | 140 | } |
---|
[255] | 141 | y.normalize(); |
---|
| 142 | z.vectorProduct(x, y); |
---|
[109] | 143 | } |
---|
| 144 | |
---|
[255] | 145 | // 2D distance |
---|
| 146 | double d2(double x, double y) |
---|
[109] | 147 | { |
---|
[255] | 148 | double q = x*x + y*y; |
---|
[375] | 149 | if (q < 0) { if (Pt3D::report_errors) logPrintf("", "d2()", LOG_ERROR, "sqrt(%g): domain error", q); return 0; } |
---|
[255] | 150 | return sqrt(q); |
---|
[109] | 151 | } |
---|
| 152 | |
---|
| 153 | Orient::Orient(const Matrix44& m) |
---|
| 154 | { |
---|
[255] | 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]; |
---|
[109] | 158 | } |
---|
| 159 | |
---|
| 160 | void Orient::operator=(const Pt3D &rot) |
---|
| 161 | { |
---|
[255] | 162 | *this = Orient_1; |
---|
| 163 | rotate(rot); |
---|
[109] | 164 | } |
---|
| 165 | |
---|
| 166 | void Orient::rotate(const Pt3D &v) |
---|
| 167 | { |
---|
[255] | 168 | double s, c; |
---|
[305] | 169 | if (v.x != 0) |
---|
[109] | 170 | { |
---|
[255] | 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); |
---|
[109] | 175 | } |
---|
[305] | 176 | if (v.y != 0) |
---|
[109] | 177 | { |
---|
[255] | 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); |
---|
[109] | 182 | } |
---|
[305] | 183 | if (v.z != 0) |
---|
[109] | 184 | { |
---|
[255] | 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); |
---|
[109] | 189 | } |
---|
| 190 | } |
---|
| 191 | |
---|
[255] | 192 | void Orient::transform(Pt3D& target, const Pt3D &s) const |
---|
[109] | 193 | { |
---|
[255] | 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; |
---|
[109] | 197 | } |
---|
| 198 | |
---|
[255] | 199 | void Orient::revTransform(Pt3D& target, const Pt3D &s) const |
---|
[109] | 200 | { |
---|
[255] | 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; |
---|
[109] | 204 | } |
---|
| 205 | |
---|
[255] | 206 | void Orient::transform(Orient& target, const Orient& src) const |
---|
[109] | 207 | { |
---|
[255] | 208 | transform(target.x, src.x); |
---|
| 209 | transform(target.y, src.y); |
---|
| 210 | transform(target.z, src.z); |
---|
[109] | 211 | } |
---|
| 212 | |
---|
[255] | 213 | void Orient::revTransform(Orient& target, const Orient& src) const |
---|
[109] | 214 | { |
---|
[255] | 215 | revTransform(target.x, src.x); |
---|
| 216 | revTransform(target.y, src.y); |
---|
| 217 | revTransform(target.z, src.z); |
---|
[109] | 218 | } |
---|
| 219 | |
---|
| 220 | void Orient::getAngles(Pt3D &angles) const |
---|
| 221 | { |
---|
[255] | 222 | angles.getAngles(x, z); |
---|
[109] | 223 | } |
---|
| 224 | |
---|
| 225 | bool Orient::normalize() |
---|
| 226 | { |
---|
[255] | 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; |
---|
[109] | 234 | } |
---|
| 235 | |
---|
| 236 | Matrix44::Matrix44(const Orient &rot) |
---|
| 237 | { |
---|
[255] | 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; |
---|
[109] | 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 | } |
---|