1 /* --------------------------------------------------------------------
2 EXTREME TUXRACER
3
4 Copyright (C) 1999-2001 Jasmin F. Patry (Tuxracer)
5 Copyright (C) 2004-2005 Volker Stroebel (Planetpenguin Racer)
6 Copyright (C) 2010 Extreme Tux Racer Team
7
8 This program is free software; you can redistribute it and/or
9 modify it under the terms of the GNU General Public License
10 as published by the Free Software Foundation; either version 2
11 of the License, or (at your option) any later version.
12
13 This program is distributed in the hope that it will be useful,
14 but WITHOUT ANY WARRANTY; without even the implied warranty of
15 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
16 GNU General Public License for more details.
17 ---------------------------------------------------------------------*/
18
19 #ifdef HAVE_CONFIG_H
20 #include <etr_config.h>
21 #endif
22
23 #include "mathlib.h"
24 #include <cstdlib>
25 #include <algorithm>
26
27
ProjectToPlane(const TVector3d & nml,const TVector3d & v)28 TVector3d ProjectToPlane(const TVector3d& nml, const TVector3d& v) {
29 double dotProd = DotProduct(nml, v);
30 TVector3d nmlComp = dotProd * nml;
31
32 return v - nmlComp;
33 }
34
35
TransformVector(const TMatrix<4,4> & mat,const TVector3d & v)36 TVector3d TransformVector(const TMatrix<4, 4>& mat, const TVector3d& v) {
37 return TVector3d(
38 v.x * mat[0][0] + v.y * mat[1][0] + v.z * mat[2][0],
39 v.x * mat[0][1] + v.y * mat[1][1] + v.z * mat[2][1],
40 v.x * mat[0][2] + v.y * mat[1][2] + v.z * mat[2][2]);
41 }
42
TransformNormal(const TVector3d & n,const TMatrix<4,4> & mat)43 TVector3d TransformNormal(const TVector3d& n, const TMatrix<4, 4>& mat) {
44 return TVector3d(
45 n.x * mat[0][0] + n.y * mat[0][1] + n.z * mat[0][2],
46 n.x * mat[1][0] + n.y * mat[1][1] + n.z * mat[1][2],
47 n.x * mat[2][0] + n.y * mat[2][1] + n.z * mat[2][2]);
48 }
49
TransformPoint(const TMatrix<4,4> & mat,const TVector3d & p)50 TVector3d TransformPoint(const TMatrix<4, 4>& mat, const TVector3d& p) {
51 TVector3d r;
52 r.x = p.x * mat[0][0] + p.y * mat[1][0] + p.z * mat[2][0];
53 r.y = p.x * mat[0][1] + p.y * mat[1][1] + p.z * mat[2][1];
54 r.z = p.x * mat[0][2] + p.y * mat[1][2] + p.z * mat[2][2];
55 r.x += mat[3][0];
56 r.y += mat[3][1];
57 r.z += mat[3][2];
58 return r;
59 }
60
IntersectPlanes(const TPlane & s1,const TPlane & s2,const TPlane & s3,TVector3d * p)61 bool IntersectPlanes(const TPlane& s1, const TPlane& s2, const TPlane& s3, TVector3d *p) {
62 double A[3][4];
63 double x[3];
64 double retval;
65
66 A[0][0] = s1.nml.x;
67 A[0][1] = s1.nml.y;
68 A[0][2] = s1.nml.z;
69 A[0][3] = -s1.d;
70
71 A[1][0] = s2.nml.x;
72 A[1][1] = s2.nml.y;
73 A[1][2] = s2.nml.z;
74 A[1][3] = -s2.d;
75
76 A[2][0] = s3.nml.x;
77 A[2][1] = s3.nml.y;
78 A[2][2] = s3.nml.z;
79 A[2][3] = -s3.d;
80
81 retval = Gauss((double*) A, 3, x);
82
83 if (retval != 0) {
84 return false;
85 } else {
86 p->x = x[0];
87 p->y = x[1];
88 p->z = x[2];
89 return true;
90 }
91 }
92
DistanceToPlane(const TPlane & plane,const TVector3d & pt)93 double DistanceToPlane(const TPlane& plane, const TVector3d& pt) {
94 return
95 DotProduct(plane.nml, pt) +
96 plane.d;
97 }
98
99
RotateAboutVectorMatrix(const TVector3d & u,double angle)100 TMatrix<4, 4> RotateAboutVectorMatrix(const TVector3d& u, double angle) {
101 TMatrix<4, 4> rx, irx, ry, iry;
102 TMatrix<4, 4> mat;
103
104 double a = u.x;
105 double b = u.y;
106 double c = u.z;
107
108 double d = std::hypot(b, c);
109
110 if (d < EPS) {
111 if (a < 0)
112 mat.SetRotationMatrix(-angle, 'x');
113 else
114 mat.SetRotationMatrix(angle, 'x');
115 return mat;
116 }
117
118 rx.SetIdentity();
119 irx.SetIdentity();
120 ry.SetIdentity();
121 iry.SetIdentity();
122
123 rx[1][1] = c/d;
124 rx[2][1] = -b/d;
125 rx[1][2] = b/d;
126 rx[2][2] = c/d;
127
128 irx[1][1] = c/d;
129 irx[2][1] = b/d;
130 irx[1][2] = -b/d;
131 irx[2][2] = c/d;
132
133 ry[0][0] = d;
134 ry[2][0] = -a;
135 ry[0][2] = a;
136 ry[2][2] = d;
137
138 iry[0][0] = d;
139 iry[2][0] = a;
140 iry[0][2] = -a;
141 iry[2][2] = d;
142
143 mat.SetRotationMatrix(angle, 'z');
144
145 mat = mat * ry;
146 mat = mat * rx;
147 mat = iry * mat;
148 mat = irx * mat;
149 return mat;
150 }
151
MultiplyQuaternions(const TQuaternion & q,const TQuaternion & r)152 TQuaternion MultiplyQuaternions(const TQuaternion& q, const TQuaternion& r) {
153 return TQuaternion(
154 q.y * r.z - q.z * r.y + r.w * q.x + q.w * r.x,
155 q.z * r.x - q.x * r.z + r.w * q.y + q.w * r.y,
156 q.x * r.y - q.y * r.x + r.w * q.z + q.w * r.z,
157 q.w * r.w - q.x * r.x - q.y * r.y - q.z * r.z);
158 }
159
ConjugateQuaternion(const TQuaternion & q)160 TQuaternion ConjugateQuaternion(const TQuaternion& q) {
161 return TQuaternion(
162 -q.x,
163 -q.y,
164 -q.z,
165 q.w);
166 }
167
MakeMatrixFromQuaternion(const TQuaternion & q)168 TMatrix<4, 4> MakeMatrixFromQuaternion(const TQuaternion& q) {
169 TMatrix<4, 4> mat;
170 mat[0][0] = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
171 mat[1][0] = 2.0 * (q.x * q.y - q.w * q.z);
172 mat[2][0] = 2.0 * (q.x * q.z + q.w * q.y);
173
174 mat[0][1] = 2.0 * (q.x * q.y + q.w * q.z);
175 mat[1][1] = 1.0 - 2.0 * (q.x * q.x + q.z * q.z);
176 mat[2][1] = 2.0 * (q.y * q.z - q.w * q.x);
177
178 mat[0][2] = 2.0 * (q.x * q.z - q.w * q.y);
179 mat[1][2] = 2.0 * (q.y * q.z + q.w * q.x);
180 mat[2][2] = 1.0 - 2.0 * (q.x * q.x + q.y * q.y);
181
182 mat[3][0] = mat[3][1] = mat[3][2] = 0.0;
183 mat[0][3] = mat[1][3] = mat[2][3] = 0.0;
184 mat[3][3] = 1.0;
185 return mat;
186 }
187
MakeQuaternionFromMatrix(const TMatrix<4,4> & m)188 TQuaternion MakeQuaternionFromMatrix(const TMatrix<4, 4>& m) {
189 TQuaternion res;
190 double tr, s, q[4];
191
192 static int nxt[3] = {1, 2, 0};
193
194 tr = m[0][0] + m[1][1] + m[2][2];
195
196 if (tr > 0.0) {
197 s = std::sqrt(tr + 1.0);
198 res.w = 0.5 * s;
199 s = 0.5 / s;
200 res.x = (m[1][2] - m[2][1]) * s;
201 res.y = (m[2][0] - m[0][2]) * s;
202 res.z = (m[0][1] - m[1][0]) * s;
203 } else {
204 int i = 0;
205 if (m[1][1] > m[0][0]) i = 1;
206 if (m[2][2] > m[i][i]) i = 2;
207 int j = nxt[i];
208 int k = nxt[j];
209
210 s = std::sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
211
212 q[i] = s * 0.5;
213
214 if (s != 0.0) s = 0.5 / s;
215
216 q[3] = (m[j][k] - m[k][j]) * s;
217 q[j] = (m[i][j] + m[j][i]) * s;
218 q[k] = (m[i][k] + m[k][i]) * s;
219
220 res.x = q[0];
221 res.y = q[1];
222 res.z = q[2];
223 res.w = q[3];
224 }
225
226 return res;
227 }
228
MakeRotationQuaternion(const TVector3d & s,const TVector3d & t)229 TQuaternion MakeRotationQuaternion(const TVector3d& s, const TVector3d& t) {
230 TVector3d u = CrossProduct(s, t);
231 double sin2phi = u.Norm();
232
233 if (sin2phi < EPS) {
234 return TQuaternion(0., 0., 0., 1.);
235 } else {
236 double cos2phi = DotProduct(s, t);
237
238 double sinphi = std::sqrt((1 - cos2phi) / 2.0);
239 double cosphi = std::sqrt((1 + cos2phi) / 2.0);
240
241 return TQuaternion(
242 sinphi * u.x,
243 sinphi * u.y,
244 sinphi * u.z,
245 cosphi);
246 }
247 }
248
InterpolateQuaternions(const TQuaternion & q,TQuaternion r,double t)249 TQuaternion InterpolateQuaternions(const TQuaternion& q, TQuaternion r, double t) {
250 double cosphi = DotProduct(q, r);
251
252 if (cosphi < 0.0) {
253 cosphi = -cosphi;
254 r.x = -r.x;
255 r.y = -r.y;
256 r.z = -r.z;
257 r.w = -r.w;
258 }
259
260 double scale0, scale1;
261 if (1.0 - cosphi > EPS) {
262 double phi = std::acos(cosphi);
263 double sinphi = std::sin(phi);
264 scale0 = std::sin(phi * (1.0 - t)) / sinphi;
265 scale1 = std::sin(phi * t) / sinphi;
266 } else {
267 scale0 = 1.0 - t;
268 scale1 = t;
269 }
270
271 return scale0 * q + scale1 * r;
272 }
273
RotateVector(const TQuaternion & q,const TVector3d & v)274 TVector3d RotateVector(const TQuaternion& q, const TVector3d& v) {
275 TQuaternion p(v.x, v.y, v.z, 1.0);
276
277 TQuaternion qs(-q.x, -q.y, -q.z, q.w);
278
279 TQuaternion res_q = MultiplyQuaternions(q, MultiplyQuaternions(p, qs));
280
281 return TVector3d(res_q.x, res_q.y, res_q.z);
282 }
283
284 // --------------------------------------------------------------------
285 // Gauss
286 // --------------------------------------------------------------------
287
288 bool order(double *matrix, int n, int pivot);
289 void elim(double *matrix, int n, int pivot);
290 void backsb(double *matrix, int n, double *soln);
291
Gauss(double * matrix,int n,double * soln)292 int Gauss(double *matrix, int n, double *soln) {
293 int pivot = 0;
294 bool error = false;
295
296 while ((pivot<(n-1)) && (!error)) {
297 error = order(matrix, n, pivot);
298 if (!error) {
299 elim(matrix,n,pivot);
300 pivot++;
301 }
302 }
303 if (error) {
304 return 1;
305 } else {
306 backsb(matrix, n, soln);
307 }
308 return 0;
309 }
310
order(double * matrix,int n,int pivot)311 bool order(double *matrix, int n, int pivot) {
312 bool error = false;
313
314 int rmax = pivot;
315
316 for (int row=pivot+1; row<n; row++) {
317 if (std::fabs(*(matrix+row*(n+1)+pivot)) > std::fabs(*(matrix+rmax*(n+1)+pivot)))
318 rmax = row;
319 }
320
321 if (std::fabs(*(matrix+rmax*(n+1)+pivot)) < EPS)
322 error = true;
323 else if (rmax != pivot) {
324 for (int k=0; k<(n+1); k++) {
325 double temp = *(matrix+rmax*(n+1)+k);
326 *(matrix+rmax*(n+1)+k) = *(matrix+pivot*(n+1)+k);
327 *(matrix+pivot*(n+1)+k) = temp;
328 }
329 }
330 return error;
331 }
332
elim(double * matrix,int n,int pivot)333 void elim(double *matrix, int n, int pivot) {
334 for (int row = pivot+1; row < n; row++) {
335 double factor = (*(matrix+row*(n+1)+pivot))/(*(matrix+pivot*(n+1)+pivot));
336 *(matrix+row*(n+1)+pivot)=0.0;
337 for (int col=pivot+1l; col<n+1; col++) {
338 *(matrix+row*(n+1)+col) = *(matrix+row*(n+1)+col) -
339 (*(matrix+pivot*(n+1)+col))*factor;
340 }
341 }
342 }
343
344
backsb(double * matrix,int n,double * soln)345 void backsb(double *matrix, int n, double *soln) {
346 for (int row = n-1; row >=0; row--) {
347 for (int col = n-1; col >= row+1; col--) {
348 *(matrix+row*(n+1)+(n)) = *(matrix+row*(n+1)+n) -
349 (*(soln+col))*(*(matrix+row*(n+1)+col));
350 }
351 *(soln+row) = (*(matrix+row*(n+1)+n))/(*(matrix+row*(n+1)+row));
352 }
353 }
354
355 // ***************************************************************************
356 // ***************************************************************************
357
IntersectPolygon(const TPolygon & p,std::vector<TVector3d> & v)358 bool IntersectPolygon(const TPolygon& p, std::vector<TVector3d>& v) {
359 TRay ray;
360 double d, s, nuDotProd;
361 double distsq;
362
363 TVector3d nml = MakeNormal(p, &v[0]);
364 ray.pt = TVector3d();
365 ray.vec = nml;
366
367 nuDotProd = DotProduct(nml, ray.vec);
368 if (std::fabs(nuDotProd) < EPS)
369 return false;
370
371 d = - DotProduct(nml, v[p.vertices[0]]);
372
373 if (std::fabs(d) > 1) return false;
374
375 for (std::size_t i=0; i < p.vertices.size(); i++) {
376 TVector3d *v0, *v1;
377
378 v0 = &v[p.vertices[i]];
379 v1 = &v[p.vertices[(i + 1) % p.vertices.size()]];
380
381 TVector3d edge_vec = *v1 - *v0;
382 double edge_len = edge_vec.Norm();
383
384 double t = - DotProduct(*v0, edge_vec);
385
386 if (t < 0) {
387 distsq = MAG_SQD(*v0);
388 } else if (t > edge_len) {
389 distsq = MAG_SQD(*v1);
390 } else {
391 *v0 += t * edge_vec;
392 distsq = MAG_SQD(*v0);
393 }
394
395 if (distsq <= 1) return true;
396 }
397
398 s = - (d + DotProduct(nml, ray.pt)) / nuDotProd;
399 TVector3d pt = ray.pt + s * ray.vec;
400
401 for (std::size_t i = 0; i < p.vertices.size(); i++) {
402 TVector3d edge_nml = CrossProduct(nml,
403 v[p.vertices[(i + 1) % p.vertices.size()]] - v[p.vertices[i]]);
404
405 double wec = DotProduct(pt - v[p.vertices[i]], edge_nml);
406 if (wec < 0) return false;
407 }
408 return true;
409 }
410
IntersectPolyhedron(TPolyhedron & p)411 bool IntersectPolyhedron(TPolyhedron& p) {
412 bool hit = false;
413 for (std::size_t i = 0; i < p.polygons.size(); i++) {
414 hit = IntersectPolygon(p.polygons[i], p.vertices);
415 if (hit == true) break;
416 }
417 return hit;
418 }
419
MakeNormal(const TPolygon & p,const TVector3d * v)420 TVector3d MakeNormal(const TPolygon& p, const TVector3d *v) {
421 TVector3d v1 = v[p.vertices[1]] - v[p.vertices[0]];
422 TVector3d v2 = v[p.vertices[p.vertices.size() - 1]] - v[p.vertices[0]];
423 TVector3d normal = CrossProduct(v1, v2);
424
425 normal.Norm();
426 return normal;
427 }
428
429
TransPolyhedron(const TMatrix<4,4> & mat,TPolyhedron & ph)430 void TransPolyhedron(const TMatrix<4, 4>& mat, TPolyhedron& ph) {
431 for (std::size_t i = 0; i < ph.vertices.size(); i++)
432 ph.vertices[i] = TransformPoint(mat, ph.vertices[i]);
433 }
434
435 // --------------------------------------------------------------------
436 // ode solver
437 // --------------------------------------------------------------------
438
439 const double ode23_time_step_mat[] = { 0., 1./2., 3./4., 1. };
440 const double ode23_coeff_mat[][4] = {
441 {0.0, 1./2., 0.0, 2./9.},
442 {0.0, 0.0, 3./4., 1./3.},
443 {0.0, 0.0, 0.0, 4./9.},
444 {0.0, 0.0, 0.0, 0.0}
445 };
446
447 const double ode23_error_mat[] = {-5./72., 1./12., 1./9., -1./8. };
448 const double ode23_time_step_exp = 1./3.;
449
ode23_NumEstimates()450 int ode23_NumEstimates() {return 4; }
451
ode23_InitOdeData(TOdeData * data,double init_val,double h)452 void ode23_InitOdeData(TOdeData *data, double init_val, double h) {
453 data->init_val = init_val;
454 data->h = h;
455 }
456
ode23_NextTime(TOdeData * data,int step)457 double ode23_NextTime(TOdeData *data, int step) {
458 return ode23_time_step_mat[step] * data->h;
459 }
460
ode23_NextValue(TOdeData * data,int step)461 double ode23_NextValue(TOdeData *data, int step) {
462 double val = data->init_val;
463
464 for (int i=0; i<step; i++)
465 val += ode23_coeff_mat[i][step] * data->k[i];
466 return val;
467 }
468
ode23_UpdateEstimate(TOdeData * data,int step,double val)469 void ode23_UpdateEstimate(TOdeData *data, int step, double val) {
470 data->k[step] = data->h * val;
471 }
472
ode23_FinalEstimate(TOdeData * data)473 double ode23_FinalEstimate(TOdeData *data) {
474 double val = data->init_val;
475
476 for (int i=0; i<3; i++)
477 val += ode23_coeff_mat[i][3] * data->k[i];
478 return val;
479 }
480
ode23_EstimateError(TOdeData * data)481 double ode23_EstimateError(TOdeData *data) {
482 double err=0.;
483
484 for (int i=0; i<4; i++)
485 err += ode23_error_mat[i] * data->k[i];
486 return std::fabs(err);
487 }
488
ode23_TimestepExponent()489 double ode23_TimestepExponent() {
490 return ode23_time_step_exp;
491 }
492
TOdeSolver()493 TOdeSolver::TOdeSolver() {
494 NumEstimates = ode23_NumEstimates;
495 InitOdeData = ode23_InitOdeData;
496 NextTime = ode23_NextTime;
497 NextValue = ode23_NextValue;
498 UpdateEstimate = ode23_UpdateEstimate;
499 FinalEstimate = ode23_FinalEstimate;
500 EstimateError = ode23_EstimateError;
501 TimestepExponent = ode23_TimestepExponent;
502 }
503
LinearInterp(const double x[],const double y[],double val,int n)504 double LinearInterp(const double x[], const double y[], double val, int n) {
505 int i;
506 double m, b;
507
508 if (val < x[0]) i = 0;
509 else if (val >= x[n-1]) i = n-2;
510 else for (i=0; i<n-1; i++) if (val < x[i+1]) break;
511
512 m = (y[i+1] - y[i]) / (x[i+1] - x[i]);
513 b = y[i] - m * x[i];
514
515 return m * val + b;
516 }
517
XRandom(double min,double max)518 double XRandom(double min, double max) {
519 return (double)std::rand() / RAND_MAX * (max - min) + min;
520 }
521
FRandom()522 double FRandom() {
523 return (double)std::rand() / RAND_MAX;
524 }
525
IRandom(int min,int max)526 int IRandom(int min, int max) {
527 return min + std::rand()%(max-min+1);
528 }
529
ITrunc(int val,int base)530 int ITrunc(int val, int base) {
531 return (int)(val / base);
532 }
533
IFrac(int val,int base)534 int IFrac(int val, int base) {
535 return val - ITrunc(val, base) * base;
536 }
537