537 lines
13 KiB
C++
537 lines
13 KiB
C++
/* --------------------------------------------------------------------
|
|
EXTREME TUXRACER
|
|
|
|
Copyright (C) 1999-2001 Jasmin F. Patry (Tuxracer)
|
|
Copyright (C) 2004-2005 Volker Stroebel (Planetpenguin Racer)
|
|
Copyright (C) 2010 Extreme Tuxracer Team
|
|
|
|
This program is free software; you can redistribute it and/or
|
|
modify it under the terms of the GNU General Public License
|
|
as published by the Free Software Foundation; either version 2
|
|
of the License, or (at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU General Public License for more details.
|
|
---------------------------------------------------------------------*/
|
|
|
|
#ifdef HAVE_CONFIG_H
|
|
#include <etr_config.h>
|
|
#endif
|
|
|
|
#include "mathlib.h"
|
|
#include <cstdlib>
|
|
#include <algorithm>
|
|
|
|
|
|
TVector3d ProjectToPlane(const TVector3d& nml, const TVector3d& v) {
|
|
double dotProd = DotProduct(nml, v);
|
|
TVector3d nmlComp = dotProd * nml;
|
|
|
|
return v - nmlComp;
|
|
}
|
|
|
|
|
|
TVector3d TransformVector(const TMatrix<4, 4>& mat, const TVector3d& v) {
|
|
return TVector3d(
|
|
v.x * mat[0][0] + v.y * mat[1][0] + v.z * mat[2][0],
|
|
v.x * mat[0][1] + v.y * mat[1][1] + v.z * mat[2][1],
|
|
v.x * mat[0][2] + v.y * mat[1][2] + v.z * mat[2][2]);
|
|
}
|
|
|
|
TVector3d TransformNormal(const TVector3d& n, const TMatrix<4, 4>& mat) {
|
|
return TVector3d(
|
|
n.x * mat[0][0] + n.y * mat[0][1] + n.z * mat[0][2],
|
|
n.x * mat[1][0] + n.y * mat[1][1] + n.z * mat[1][2],
|
|
n.x * mat[2][0] + n.y * mat[2][1] + n.z * mat[2][2]);
|
|
}
|
|
|
|
TVector3d TransformPoint(const TMatrix<4, 4>& mat, const TVector3d& p) {
|
|
TVector3d r;
|
|
r.x = p.x * mat[0][0] + p.y * mat[1][0] + p.z * mat[2][0];
|
|
r.y = p.x * mat[0][1] + p.y * mat[1][1] + p.z * mat[2][1];
|
|
r.z = p.x * mat[0][2] + p.y * mat[1][2] + p.z * mat[2][2];
|
|
r.x += mat[3][0];
|
|
r.y += mat[3][1];
|
|
r.z += mat[3][2];
|
|
return r;
|
|
}
|
|
|
|
bool IntersectPlanes(const TPlane& s1, const TPlane& s2, const TPlane& s3, TVector3d *p) {
|
|
double A[3][4];
|
|
double x[3];
|
|
double retval;
|
|
|
|
A[0][0] = s1.nml.x;
|
|
A[0][1] = s1.nml.y;
|
|
A[0][2] = s1.nml.z;
|
|
A[0][3] = -s1.d;
|
|
|
|
A[1][0] = s2.nml.x;
|
|
A[1][1] = s2.nml.y;
|
|
A[1][2] = s2.nml.z;
|
|
A[1][3] = -s2.d;
|
|
|
|
A[2][0] = s3.nml.x;
|
|
A[2][1] = s3.nml.y;
|
|
A[2][2] = s3.nml.z;
|
|
A[2][3] = -s3.d;
|
|
|
|
retval = Gauss((double*) A, 3, x);
|
|
|
|
if (retval != 0) {
|
|
return false;
|
|
} else {
|
|
p->x = x[0];
|
|
p->y = x[1];
|
|
p->z = x[2];
|
|
return true;
|
|
}
|
|
}
|
|
|
|
double DistanceToPlane(const TPlane& plane, const TVector3d& pt) {
|
|
return
|
|
DotProduct(plane.nml, pt) +
|
|
plane.d;
|
|
}
|
|
|
|
|
|
TMatrix<4, 4> RotateAboutVectorMatrix(const TVector3d& u, double angle) {
|
|
TMatrix<4, 4> rx, irx, ry, iry;
|
|
TMatrix<4, 4> mat;
|
|
|
|
double a = u.x;
|
|
double b = u.y;
|
|
double c = u.z;
|
|
|
|
double d = std::hypot(b, c);
|
|
|
|
if (d < EPS) {
|
|
if (a < 0)
|
|
mat.SetRotationMatrix(-angle, 'x');
|
|
else
|
|
mat.SetRotationMatrix(angle, 'x');
|
|
return mat;
|
|
}
|
|
|
|
rx.SetIdentity();
|
|
irx.SetIdentity();
|
|
ry.SetIdentity();
|
|
iry.SetIdentity();
|
|
|
|
rx[1][1] = c/d;
|
|
rx[2][1] = -b/d;
|
|
rx[1][2] = b/d;
|
|
rx[2][2] = c/d;
|
|
|
|
irx[1][1] = c/d;
|
|
irx[2][1] = b/d;
|
|
irx[1][2] = -b/d;
|
|
irx[2][2] = c/d;
|
|
|
|
ry[0][0] = d;
|
|
ry[2][0] = -a;
|
|
ry[0][2] = a;
|
|
ry[2][2] = d;
|
|
|
|
iry[0][0] = d;
|
|
iry[2][0] = a;
|
|
iry[0][2] = -a;
|
|
iry[2][2] = d;
|
|
|
|
mat.SetRotationMatrix(angle, 'z');
|
|
|
|
mat = mat * ry;
|
|
mat = mat * rx;
|
|
mat = iry * mat;
|
|
mat = irx * mat;
|
|
return mat;
|
|
}
|
|
|
|
TQuaternion MultiplyQuaternions(const TQuaternion& q, const TQuaternion& r) {
|
|
return TQuaternion(
|
|
q.y * r.z - q.z * r.y + r.w * q.x + q.w * r.x,
|
|
q.z * r.x - q.x * r.z + r.w * q.y + q.w * r.y,
|
|
q.x * r.y - q.y * r.x + r.w * q.z + q.w * r.z,
|
|
q.w * r.w - q.x * r.x - q.y * r.y - q.z * r.z);
|
|
}
|
|
|
|
TQuaternion ConjugateQuaternion(const TQuaternion& q) {
|
|
return TQuaternion(
|
|
-q.x,
|
|
-q.y,
|
|
-q.z,
|
|
q.w);
|
|
}
|
|
|
|
TMatrix<4, 4> MakeMatrixFromQuaternion(const TQuaternion& q) {
|
|
TMatrix<4, 4> mat;
|
|
mat[0][0] = 1.0 - 2.0 * (q.y * q.y + q.z * q.z);
|
|
mat[1][0] = 2.0 * (q.x * q.y - q.w * q.z);
|
|
mat[2][0] = 2.0 * (q.x * q.z + q.w * q.y);
|
|
|
|
mat[0][1] = 2.0 * (q.x * q.y + q.w * q.z);
|
|
mat[1][1] = 1.0 - 2.0 * (q.x * q.x + q.z * q.z);
|
|
mat[2][1] = 2.0 * (q.y * q.z - q.w * q.x);
|
|
|
|
mat[0][2] = 2.0 * (q.x * q.z - q.w * q.y);
|
|
mat[1][2] = 2.0 * (q.y * q.z + q.w * q.x);
|
|
mat[2][2] = 1.0 - 2.0 * (q.x * q.x + q.y * q.y);
|
|
|
|
mat[3][0] = mat[3][1] = mat[3][2] = 0.0;
|
|
mat[0][3] = mat[1][3] = mat[2][3] = 0.0;
|
|
mat[3][3] = 1.0;
|
|
return mat;
|
|
}
|
|
|
|
TQuaternion MakeQuaternionFromMatrix(const TMatrix<4, 4>& m) {
|
|
TQuaternion res;
|
|
double tr, s, q[4];
|
|
|
|
static int nxt[3] = {1, 2, 0};
|
|
|
|
tr = m[0][0] + m[1][1] + m[2][2];
|
|
|
|
if (tr > 0.0) {
|
|
s = std::sqrt(tr + 1.0);
|
|
res.w = 0.5 * s;
|
|
s = 0.5 / s;
|
|
res.x = (m[1][2] - m[2][1]) * s;
|
|
res.y = (m[2][0] - m[0][2]) * s;
|
|
res.z = (m[0][1] - m[1][0]) * s;
|
|
} else {
|
|
int i = 0;
|
|
if (m[1][1] > m[0][0]) i = 1;
|
|
if (m[2][2] > m[i][i]) i = 2;
|
|
int j = nxt[i];
|
|
int k = nxt[j];
|
|
|
|
s = std::sqrt(m[i][i] - m[j][j] - m[k][k] + 1.0);
|
|
|
|
q[i] = s * 0.5;
|
|
|
|
if (s != 0.0) s = 0.5 / s;
|
|
|
|
q[3] = (m[j][k] - m[k][j]) * s;
|
|
q[j] = (m[i][j] + m[j][i]) * s;
|
|
q[k] = (m[i][k] + m[k][i]) * s;
|
|
|
|
res.x = q[0];
|
|
res.y = q[1];
|
|
res.z = q[2];
|
|
res.w = q[3];
|
|
}
|
|
|
|
return res;
|
|
}
|
|
|
|
TQuaternion MakeRotationQuaternion(const TVector3d& s, const TVector3d& t) {
|
|
TVector3d u = CrossProduct(s, t);
|
|
double sin2phi = u.Norm();
|
|
|
|
if (sin2phi < EPS) {
|
|
return TQuaternion(0., 0., 0., 1.);
|
|
} else {
|
|
double cos2phi = DotProduct(s, t);
|
|
|
|
double sinphi = std::sqrt((1 - cos2phi) / 2.0);
|
|
double cosphi = std::sqrt((1 + cos2phi) / 2.0);
|
|
|
|
return TQuaternion(
|
|
sinphi * u.x,
|
|
sinphi * u.y,
|
|
sinphi * u.z,
|
|
cosphi);
|
|
}
|
|
}
|
|
|
|
TQuaternion InterpolateQuaternions(const TQuaternion& q, TQuaternion r, double t) {
|
|
double cosphi = DotProduct(q, r);
|
|
|
|
if (cosphi < 0.0) {
|
|
cosphi = -cosphi;
|
|
r.x = -r.x;
|
|
r.y = -r.y;
|
|
r.z = -r.z;
|
|
r.w = -r.w;
|
|
}
|
|
|
|
double scale0, scale1;
|
|
if (1.0 - cosphi > EPS) {
|
|
double phi = std::acos(cosphi);
|
|
double sinphi = std::sin(phi);
|
|
scale0 = std::sin(phi * (1.0 - t)) / sinphi;
|
|
scale1 = std::sin(phi * t) / sinphi;
|
|
} else {
|
|
scale0 = 1.0 - t;
|
|
scale1 = t;
|
|
}
|
|
|
|
return scale0 * q + scale1 * r;
|
|
}
|
|
|
|
TVector3d RotateVector(const TQuaternion& q, const TVector3d& v) {
|
|
TQuaternion p(v.x, v.y, v.z, 1.0);
|
|
|
|
TQuaternion qs(-q.x, -q.y, -q.z, q.w);
|
|
|
|
TQuaternion res_q = MultiplyQuaternions(q, MultiplyQuaternions(p, qs));
|
|
|
|
return TVector3d(res_q.x, res_q.y, res_q.z);
|
|
}
|
|
|
|
// --------------------------------------------------------------------
|
|
// Gauss
|
|
// --------------------------------------------------------------------
|
|
|
|
bool order(double *matrix, int n, int pivot);
|
|
void elim(double *matrix, int n, int pivot);
|
|
void backsb(double *matrix, int n, double *soln);
|
|
|
|
int Gauss(double *matrix, int n, double *soln) {
|
|
int pivot = 0;
|
|
bool error = false;
|
|
|
|
while ((pivot<(n-1)) && (!error)) {
|
|
error = order(matrix, n, pivot);
|
|
if (!error) {
|
|
elim(matrix,n,pivot);
|
|
pivot++;
|
|
}
|
|
}
|
|
if (error) {
|
|
return 1;
|
|
} else {
|
|
backsb(matrix, n, soln);
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
bool order(double *matrix, int n, int pivot) {
|
|
bool error = false;
|
|
|
|
int rmax = pivot;
|
|
|
|
for (int row=pivot+1; row<n; row++) {
|
|
if (std::fabs(*(matrix+row*(n+1)+pivot)) > std::fabs(*(matrix+rmax*(n+1)+pivot)))
|
|
rmax = row;
|
|
}
|
|
|
|
if (std::fabs(*(matrix+rmax*(n+1)+pivot)) < EPS)
|
|
error = true;
|
|
else if (rmax != pivot) {
|
|
for (int k=0; k<(n+1); k++) {
|
|
double temp = *(matrix+rmax*(n+1)+k);
|
|
*(matrix+rmax*(n+1)+k) = *(matrix+pivot*(n+1)+k);
|
|
*(matrix+pivot*(n+1)+k) = temp;
|
|
}
|
|
}
|
|
return error;
|
|
}
|
|
|
|
void elim(double *matrix, int n, int pivot) {
|
|
for (int row = pivot+1; row < n; row++) {
|
|
double factor = (*(matrix+row*(n+1)+pivot))/(*(matrix+pivot*(n+1)+pivot));
|
|
*(matrix+row*(n+1)+pivot)=0.0;
|
|
for (int col=pivot+1l; col<n+1; col++) {
|
|
*(matrix+row*(n+1)+col) = *(matrix+row*(n+1)+col) -
|
|
(*(matrix+pivot*(n+1)+col))*factor;
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
void backsb(double *matrix, int n, double *soln) {
|
|
for (int row = n-1; row >=0; row--) {
|
|
for (int col = n-1; col >= row+1; col--) {
|
|
*(matrix+row*(n+1)+(n)) = *(matrix+row*(n+1)+n) -
|
|
(*(soln+col))*(*(matrix+row*(n+1)+col));
|
|
}
|
|
*(soln+row) = (*(matrix+row*(n+1)+n))/(*(matrix+row*(n+1)+row));
|
|
}
|
|
}
|
|
|
|
// ***************************************************************************
|
|
// ***************************************************************************
|
|
|
|
bool IntersectPolygon(const TPolygon& p, std::vector<TVector3d>& v) {
|
|
TRay ray;
|
|
double d, s, nuDotProd;
|
|
double distsq;
|
|
|
|
TVector3d nml = MakeNormal(p, &v[0]);
|
|
ray.pt = TVector3d();
|
|
ray.vec = nml;
|
|
|
|
nuDotProd = DotProduct(nml, ray.vec);
|
|
if (std::fabs(nuDotProd) < EPS)
|
|
return false;
|
|
|
|
d = - DotProduct(nml, v[p.vertices[0]]);
|
|
|
|
if (std::fabs(d) > 1) return false;
|
|
|
|
for (std::size_t i=0; i < p.vertices.size(); i++) {
|
|
TVector3d *v0, *v1;
|
|
|
|
v0 = &v[p.vertices[i]];
|
|
v1 = &v[p.vertices[(i + 1) % p.vertices.size()]];
|
|
|
|
TVector3d edge_vec = *v1 - *v0;
|
|
double edge_len = edge_vec.Norm();
|
|
|
|
double t = - DotProduct(*v0, edge_vec);
|
|
|
|
if (t < 0) {
|
|
distsq = MAG_SQD(*v0);
|
|
} else if (t > edge_len) {
|
|
distsq = MAG_SQD(*v1);
|
|
} else {
|
|
*v0 += t * edge_vec;
|
|
distsq = MAG_SQD(*v0);
|
|
}
|
|
|
|
if (distsq <= 1) return true;
|
|
}
|
|
|
|
s = - (d + DotProduct(nml, ray.pt)) / nuDotProd;
|
|
TVector3d pt = ray.pt + s * ray.vec;
|
|
|
|
for (std::size_t i = 0; i < p.vertices.size(); i++) {
|
|
TVector3d edge_nml = CrossProduct(nml,
|
|
v[p.vertices[(i + 1) % p.vertices.size()]] - v[p.vertices[i]]);
|
|
|
|
double wec = DotProduct(pt - v[p.vertices[i]], edge_nml);
|
|
if (wec < 0) return false;
|
|
}
|
|
return true;
|
|
}
|
|
|
|
bool IntersectPolyhedron(TPolyhedron& p) {
|
|
bool hit = false;
|
|
for (std::size_t i = 0; i < p.polygons.size(); i++) {
|
|
hit = IntersectPolygon(p.polygons[i], p.vertices);
|
|
if (hit == true) break;
|
|
}
|
|
return hit;
|
|
}
|
|
|
|
TVector3d MakeNormal(const TPolygon& p, const TVector3d *v) {
|
|
TVector3d v1 = v[p.vertices[1]] - v[p.vertices[0]];
|
|
TVector3d v2 = v[p.vertices[p.vertices.size() - 1]] - v[p.vertices[0]];
|
|
TVector3d normal = CrossProduct(v1, v2);
|
|
|
|
normal.Norm();
|
|
return normal;
|
|
}
|
|
|
|
|
|
void TransPolyhedron(const TMatrix<4, 4>& mat, TPolyhedron& ph) {
|
|
for (std::size_t i = 0; i < ph.vertices.size(); i++)
|
|
ph.vertices[i] = TransformPoint(mat, ph.vertices[i]);
|
|
}
|
|
|
|
// --------------------------------------------------------------------
|
|
// ode solver
|
|
// --------------------------------------------------------------------
|
|
|
|
const double ode23_time_step_mat[] = { 0., 1./2., 3./4., 1. };
|
|
const double ode23_coeff_mat[][4] = {
|
|
{0.0, 1./2., 0.0, 2./9.},
|
|
{0.0, 0.0, 3./4., 1./3.},
|
|
{0.0, 0.0, 0.0, 4./9.},
|
|
{0.0, 0.0, 0.0, 0.0}
|
|
};
|
|
|
|
const double ode23_error_mat[] = {-5./72., 1./12., 1./9., -1./8. };
|
|
const double ode23_time_step_exp = 1./3.;
|
|
|
|
int ode23_NumEstimates() {return 4; }
|
|
|
|
void ode23_InitOdeData(TOdeData *data, double init_val, double h) {
|
|
data->init_val = init_val;
|
|
data->h = h;
|
|
}
|
|
|
|
double ode23_NextTime(TOdeData *data, int step) {
|
|
return ode23_time_step_mat[step] * data->h;
|
|
}
|
|
|
|
double ode23_NextValue(TOdeData *data, int step) {
|
|
double val = data->init_val;
|
|
|
|
for (int i=0; i<step; i++)
|
|
val += ode23_coeff_mat[i][step] * data->k[i];
|
|
return val;
|
|
}
|
|
|
|
void ode23_UpdateEstimate(TOdeData *data, int step, double val) {
|
|
data->k[step] = data->h * val;
|
|
}
|
|
|
|
double ode23_FinalEstimate(TOdeData *data) {
|
|
double val = data->init_val;
|
|
|
|
for (int i=0; i<3; i++)
|
|
val += ode23_coeff_mat[i][3] * data->k[i];
|
|
return val;
|
|
}
|
|
|
|
double ode23_EstimateError(TOdeData *data) {
|
|
double err=0.;
|
|
|
|
for (int i=0; i<4; i++)
|
|
err += ode23_error_mat[i] * data->k[i];
|
|
return std::fabs(err);
|
|
}
|
|
|
|
double ode23_TimestepExponent() {
|
|
return ode23_time_step_exp;
|
|
}
|
|
|
|
TOdeSolver::TOdeSolver() {
|
|
NumEstimates = ode23_NumEstimates;
|
|
InitOdeData = ode23_InitOdeData;
|
|
NextTime = ode23_NextTime;
|
|
NextValue = ode23_NextValue;
|
|
UpdateEstimate = ode23_UpdateEstimate;
|
|
FinalEstimate = ode23_FinalEstimate;
|
|
EstimateError = ode23_EstimateError;
|
|
TimestepExponent = ode23_TimestepExponent;
|
|
}
|
|
|
|
double LinearInterp(const double x[], const double y[], double val, int n) {
|
|
int i;
|
|
double m, b;
|
|
|
|
if (val < x[0]) i = 0;
|
|
else if (val >= x[n-1]) i = n-2;
|
|
else for (i=0; i<n-1; i++) if (val < x[i+1]) break;
|
|
|
|
m = (y[i+1] - y[i]) / (x[i+1] - x[i]);
|
|
b = y[i] - m * x[i];
|
|
|
|
return m * val + b;
|
|
}
|
|
|
|
double XRandom(double min, double max) {
|
|
return (double)std::rand() / RAND_MAX * (max - min) + min;
|
|
}
|
|
|
|
double FRandom() {
|
|
return (double)std::rand() / RAND_MAX;
|
|
}
|
|
|
|
int IRandom(int min, int max) {
|
|
return min + std::rand()%(max-min+1);
|
|
}
|
|
|
|
int ITrunc(int val, int base) {
|
|
return (int)(val / base);
|
|
}
|
|
|
|
int IFrac(int val, int base) {
|
|
return val - ITrunc(val, base) * base;
|
|
}
|