[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] | 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; |
---|
[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 | |
---|
| 23 | bool 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 | |
---|
| 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 | { |
---|
| 66 | if (report_errors) FMprintf("Pt3D", "getAngle()", FMLV_WARN, "atan2(%g,%g)", dy, dx); |
---|
| 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 | { |
---|
[255] | 77 | rotate2D(-z, t1.x, t1.y); |
---|
| 78 | rotate2D(-z, t2.x, t2.y); |
---|
[305] | 79 | y = getAngle(t1.x, t1.z); |
---|
[109] | 80 | } |
---|
[255] | 81 | else // vertical |
---|
[303] | 82 | { |
---|
[255] | 83 | z = 0; |
---|
| 84 | if (t1.z < 0) |
---|
| 85 | y = -M_PI_2; // down |
---|
| 86 | else |
---|
| 87 | y = M_PI_2; // up |
---|
[109] | 88 | } |
---|
[255] | 89 | rotate2D(-y, t2.x, t2.z); |
---|
[305] | 90 | x = getAngle(t2.z, -t2.y); |
---|
[109] | 91 | } |
---|
| 92 | |
---|
| 93 | void Pt3D::getMin(const Pt3D& p) |
---|
| 94 | { |
---|
[255] | 95 | if (p.x < x) x = p.x; |
---|
| 96 | if (p.y < y) y = p.y; |
---|
| 97 | if (p.z < z) z = p.z; |
---|
[109] | 98 | } |
---|
| 99 | void Pt3D::getMax(const Pt3D& p) |
---|
| 100 | { |
---|
[255] | 101 | if (p.x > x) x = p.x; |
---|
| 102 | if (p.y > y) y = p.y; |
---|
| 103 | if (p.z > z) z = p.z; |
---|
[109] | 104 | } |
---|
| 105 | |
---|
[255] | 106 | void Pt3D::vectorProduct(const Pt3D& a, const Pt3D& b) |
---|
[109] | 107 | { |
---|
[255] | 108 | x = a.y*b.z - a.z*b.y; |
---|
| 109 | y = a.z*b.x - a.x*b.z; |
---|
| 110 | z = a.x*b.y - a.y*b.x; |
---|
[109] | 111 | } |
---|
| 112 | |
---|
[255] | 113 | void Orient::lookAt(const Pt3D& X, const Pt3D& dir) |
---|
[109] | 114 | { |
---|
[255] | 115 | x = X; x.normalize(); |
---|
| 116 | y.vectorProduct(dir, x); |
---|
| 117 | z.vectorProduct(x, y); |
---|
| 118 | if ((!y.normalize()) || (!z.normalize())) |
---|
| 119 | lookAt(X);// dir was (nearly?) parallel, there is no good solution, use the x-only variant |
---|
| 120 | } |
---|
| 121 | |
---|
| 122 | void Orient::lookAt(const Pt3D& X) |
---|
| 123 | { |
---|
| 124 | x = X; x.normalize(); |
---|
| 125 | // "invent" y vector, not parallel to x |
---|
| 126 | double ax = fabs(x.x), ay = fabs(x.y), az = fabs(x.z); |
---|
| 127 | // find the smallest component |
---|
| 128 | if ((ax <= ay) && (ax <= az)) // x |
---|
[109] | 129 | { |
---|
[255] | 130 | y.x = 0; y.y = -x.z; y.z = x.y; // (0,-z,y) |
---|
[303] | 131 | } |
---|
[255] | 132 | if ((ay <= ax) && (ay <= az)) // y |
---|
| 133 | { |
---|
| 134 | y.x = -x.z; y.y = 0; y.z = x.x; // (-z,0,x) |
---|
[303] | 135 | } |
---|
[255] | 136 | else // z |
---|
| 137 | { |
---|
| 138 | y.x = -x.y; y.y = x.x; y.z = 0; // (-y,x,0) |
---|
[303] | 139 | } |
---|
[255] | 140 | y.normalize(); |
---|
| 141 | z.vectorProduct(x, y); |
---|
[109] | 142 | } |
---|
| 143 | |
---|
[255] | 144 | // 2D distance |
---|
| 145 | double d2(double x, double y) |
---|
[109] | 146 | { |
---|
[255] | 147 | double q = x*x + y*y; |
---|
[303] | 148 | if (q < 0) { if (Pt3D::report_errors) FMprintf("", "d2()", FMLV_ERROR, "sqrt(%g): domain error", q); return 0; } |
---|
[255] | 149 | return sqrt(q); |
---|
[109] | 150 | } |
---|
| 151 | |
---|
| 152 | Orient::Orient(const Matrix44& m) |
---|
| 153 | { |
---|
[255] | 154 | x.x = m[0]; x.y = m[1]; x.z = m[2]; |
---|
| 155 | y.x = m[4]; y.y = m[5]; y.z = m[6]; |
---|
| 156 | z.x = m[8]; z.y = m[9]; z.z = m[10]; |
---|
[109] | 157 | } |
---|
| 158 | |
---|
| 159 | void Orient::operator=(const Pt3D &rot) |
---|
| 160 | { |
---|
[255] | 161 | *this = Orient_1; |
---|
| 162 | rotate(rot); |
---|
[109] | 163 | } |
---|
| 164 | |
---|
| 165 | void Orient::rotate(const Pt3D &v) |
---|
| 166 | { |
---|
[255] | 167 | double s, c; |
---|
[305] | 168 | if (v.x != 0) |
---|
[109] | 169 | { |
---|
[255] | 170 | s = sin(v.x); c = cos(v.x); |
---|
| 171 | rotate2D(s, c, x.y, x.z); |
---|
| 172 | rotate2D(s, c, y.y, y.z); |
---|
| 173 | rotate2D(s, c, z.y, z.z); |
---|
[109] | 174 | } |
---|
[305] | 175 | if (v.y != 0) |
---|
[109] | 176 | { |
---|
[255] | 177 | s = sin(v.y); c = cos(v.y); |
---|
| 178 | rotate2D(s, c, x.x, x.z); |
---|
| 179 | rotate2D(s, c, y.x, y.z); |
---|
| 180 | rotate2D(s, c, z.x, z.z); |
---|
[109] | 181 | } |
---|
[305] | 182 | if (v.z != 0) |
---|
[109] | 183 | { |
---|
[255] | 184 | s = sin(v.z); c = cos(v.z); |
---|
| 185 | rotate2D(s, c, x.x, x.y); |
---|
| 186 | rotate2D(s, c, y.x, y.y); |
---|
| 187 | rotate2D(s, c, z.x, z.y); |
---|
[109] | 188 | } |
---|
| 189 | } |
---|
| 190 | |
---|
[255] | 191 | void Orient::transform(Pt3D& target, const Pt3D &s) const |
---|
[109] | 192 | { |
---|
[255] | 193 | target.x = s.x*x.x + s.y*y.x + s.z*z.x; |
---|
| 194 | target.y = s.x*x.y + s.y*y.y + s.z*z.y; |
---|
| 195 | target.z = s.x*x.z + s.y*y.z + s.z*z.z; |
---|
[109] | 196 | } |
---|
| 197 | |
---|
[255] | 198 | void Orient::revTransform(Pt3D& target, const Pt3D &s) const |
---|
[109] | 199 | { |
---|
[255] | 200 | target.x = s.x*x.x + s.y*x.y + s.z*x.z; |
---|
| 201 | target.y = s.x*y.x + s.y*y.y + s.z*y.z; |
---|
| 202 | target.z = s.x*z.x + s.y*z.y + s.z*z.z; |
---|
[109] | 203 | } |
---|
| 204 | |
---|
[255] | 205 | void Orient::transform(Orient& target, const Orient& src) const |
---|
[109] | 206 | { |
---|
[255] | 207 | transform(target.x, src.x); |
---|
| 208 | transform(target.y, src.y); |
---|
| 209 | transform(target.z, src.z); |
---|
[109] | 210 | } |
---|
| 211 | |
---|
[255] | 212 | void Orient::revTransform(Orient& target, const Orient& src) const |
---|
[109] | 213 | { |
---|
[255] | 214 | revTransform(target.x, src.x); |
---|
| 215 | revTransform(target.y, src.y); |
---|
| 216 | revTransform(target.z, src.z); |
---|
[109] | 217 | } |
---|
| 218 | |
---|
| 219 | void Orient::getAngles(Pt3D &angles) const |
---|
| 220 | { |
---|
[255] | 221 | angles.getAngles(x, z); |
---|
[109] | 222 | } |
---|
| 223 | |
---|
| 224 | bool Orient::normalize() |
---|
| 225 | { |
---|
[255] | 226 | bool ret = 1; |
---|
| 227 | y.vectorProduct(z, x); |
---|
| 228 | z.vectorProduct(x, y); |
---|
| 229 | if (!x.normalize()) ret = 0; |
---|
| 230 | if (!z.normalize()) ret = 0; |
---|
| 231 | if (!y.normalize()) ret = 0; |
---|
| 232 | return ret; |
---|
[109] | 233 | } |
---|
| 234 | |
---|
| 235 | Matrix44::Matrix44(const Orient &rot) |
---|
| 236 | { |
---|
[255] | 237 | m[0] = rot.x.x; m[1] = rot.x.y; m[2] = rot.x.z; m[3] = 0; |
---|
| 238 | m[4] = rot.y.x; m[5] = rot.y.y; m[6] = rot.y.z; m[7] = 0; |
---|
| 239 | m[8] = rot.z.x; m[9] = rot.z.y; m[10] = rot.z.z; m[11] = 0; |
---|
| 240 | m[12] = 0; m[13] = 0; m[14] = 0; m[15] = 1; |
---|
[109] | 241 | } |
---|
| 242 | |
---|
| 243 | void Matrix44::operator+=(const Pt3D &) |
---|
| 244 | { |
---|
| 245 | |
---|
| 246 | } |
---|
| 247 | |
---|
| 248 | void Matrix44::operator*=(const Pt3D &) |
---|
| 249 | { |
---|
| 250 | } |
---|
| 251 | |
---|
| 252 | void Matrix44::operator*=(double sc) |
---|
| 253 | { |
---|
| 254 | } |
---|