342 lines
9.8 KiB
C++
342 lines
9.8 KiB
C++
/* ColDet - C++ 3D Collision Detection Library
|
|
* Copyright (C) 2000 Amir Geva
|
|
*
|
|
* This library is free software; you can redistribute it and/or
|
|
* modify it under the terms of the GNU Library General Public
|
|
* License as published by the Free Software Foundation; either
|
|
* version 2 of the License, or (at your option) any later version.
|
|
*
|
|
* This library 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
|
|
* Library General Public License for more details.
|
|
*
|
|
* You should have received a copy of the GNU Library General Public
|
|
* License along with this library; if not, write to the
|
|
* Free Software Foundation, Inc., 59 Temple Place - Suite 330,
|
|
* Boston, MA 02111-1307, USA.
|
|
*
|
|
* Any comments, questions and bug reports send to:
|
|
* photon@photoneffect.com
|
|
*
|
|
* Or visit the home page: http://photoneffect.com/coldet/
|
|
*/
|
|
#ifndef H_MATH3D
|
|
#define H_MATH3D
|
|
|
|
#include <math.h>
|
|
|
|
struct Vector3D;
|
|
struct Matrix3;
|
|
struct Matrix3D;
|
|
struct Plane;
|
|
|
|
inline float flabs(float f) { return (f>=0.0f?f:-f); }
|
|
const float epsilon=1e-8f;
|
|
inline bool IsZero(float f) { return flabs(f)<epsilon; }
|
|
|
|
Vector3D operator*(float scalar, const Vector3D& v);
|
|
Vector3D operator*(const Vector3D & v, float scalar);
|
|
float operator*(const Vector3D& v1, const Vector3D& v2);
|
|
Vector3D operator+(const Vector3D& v1, const Vector3D& v2);
|
|
Vector3D operator-(const Vector3D& v1, const Vector3D& v2);
|
|
Vector3D CrossProduct(const Vector3D& v1, const Vector3D& v2);
|
|
Matrix3D operator*(const Matrix3D& m1, const Matrix3D& m2);
|
|
Matrix3D operator*(float scalar, const Matrix3D& m);
|
|
|
|
struct Vector3D
|
|
{
|
|
float x,y,z;
|
|
static const Vector3D Zero;
|
|
|
|
Vector3D() {}
|
|
Vector3D(float X, float Y, float Z) : x(X), y(Y), z(Z) {}
|
|
Vector3D(const Vector3D& v) : x(v.x), y(v.y), z(v.z) {}
|
|
|
|
Vector3D& operator+=(const Vector3D& v) { x+=v.x; y+=v.y; z+=v.z; return *this; }
|
|
Vector3D& operator*=(float s) { x*=s; y*=s; z*=s; return *this; }
|
|
Vector3D& operator/=(float s) { return *this *= (1.0f/s); }
|
|
bool operator==(const Vector3D& v) { return x==v.x && y==v.y && z==v.z; }
|
|
|
|
Vector3D operator- () const { return Vector3D(-x,-y,-z); }
|
|
float SquareMagnitude () const { return x*x+y*y+z*z; }
|
|
float Magnitude () const { return (float)sqrt(SquareMagnitude()); }
|
|
Vector3D Normalized () const { return (1.0f/Magnitude())*(*this); }
|
|
float operator[] (int i) const { return ((float*)&x)[i]; }
|
|
float& operator[] (int i) { return ((float*)&x)[i]; }
|
|
};
|
|
|
|
#define _11 sclr.s11
|
|
#define _12 sclr.s12
|
|
#define _13 sclr.s13
|
|
#define _14 sclr.s14
|
|
#define _21 sclr.s21
|
|
#define _22 sclr.s22
|
|
#define _23 sclr.s23
|
|
#define _24 sclr.s24
|
|
#define _31 sclr.s31
|
|
#define _32 sclr.s32
|
|
#define _33 sclr.s33
|
|
#define _34 sclr.s34
|
|
#define _41 sclr.s41
|
|
#define _42 sclr.s42
|
|
#define _43 sclr.s43
|
|
#define _44 sclr.s44
|
|
|
|
struct Matrix3
|
|
{
|
|
union {
|
|
struct { float s11,s12,s13,
|
|
s21,s22,s23,
|
|
s31,s32,s33; } sclr;
|
|
float m[3][3];
|
|
};
|
|
static const Matrix3 Identity;
|
|
|
|
//Vector3D& baseRow(int i) { return *((Vector3D*)m[i]); }
|
|
float operator() (int i, int j) const { return m[i][j]; }
|
|
float& operator() (int i, int j) { return m[i][j]; }
|
|
};
|
|
|
|
Matrix3D PitchMatrix3D(const float theta);
|
|
Matrix3D YawMatrix3D(const float theta);
|
|
Matrix3D RollMatrix3D(const float theta);
|
|
struct Matrix3D
|
|
{
|
|
union {
|
|
struct { float s11,s12,s13,s14,
|
|
s21,s22,s23,s24,
|
|
s31,s32,s33,s34,
|
|
s41,s42,s43,s44; } sclr;
|
|
float m[4][4];
|
|
};
|
|
static const Matrix3D Identity;
|
|
|
|
Matrix3D() {}
|
|
|
|
Matrix3D(float f11, float f12, float f13, float f14,
|
|
float f21, float f22, float f23, float f24,
|
|
float f31, float f32, float f33, float f34,
|
|
float f41, float f42, float f43, float f44)
|
|
{
|
|
_11=f11; _12=f12; _13=f13; _14=f14;
|
|
_21=f21; _22=f22; _23=f23; _24=f24;
|
|
_31=f31; _32=f32; _33=f33; _34=f34;
|
|
_41=f41; _42=f42; _43=f43; _44=f44;
|
|
}
|
|
|
|
Matrix3D& operator*= (const Matrix3D& m)
|
|
{
|
|
return *this = *this * m;
|
|
}
|
|
|
|
/*
|
|
friend Matrix3D PitchMatrix3D(const float theta);
|
|
friend Matrix3D YawMatrix3D(const float theta);
|
|
friend Matrix3D RollMatrix3D(const float theta);
|
|
*/
|
|
void rotate(const Vector3D& v);
|
|
|
|
Matrix3D Inverse() const;
|
|
Matrix3D Adjoint() const;
|
|
float Determinant() const;
|
|
Vector3D GetTranslate() const;
|
|
void Translate(const Vector3D & v);
|
|
|
|
float operator() (int i, int j) const { return m[i][j]; }
|
|
float& operator() (int i, int j) { return m[i][j]; }
|
|
void RotZ(float v);
|
|
};
|
|
|
|
struct Plane
|
|
{
|
|
Vector3D normal;
|
|
float d;
|
|
|
|
Plane(const Vector3D& a, const Vector3D& b, const Vector3D& c)
|
|
{
|
|
normal = CrossProduct(b - a, c - a).Normalized();
|
|
d = -normal * a;
|
|
}
|
|
|
|
float Classify(const Vector3D& v)
|
|
{
|
|
return v * normal + d;
|
|
}
|
|
};
|
|
|
|
inline Vector3D operator* (float scalar, const Vector3D& v)
|
|
{
|
|
return Vector3D(scalar*v.x,scalar*v.y,scalar*v.z);
|
|
}
|
|
|
|
inline Vector3D operator* (const Vector3D& v, float scalar)
|
|
{
|
|
return Vector3D(scalar*v.x,scalar*v.y,scalar*v.z);
|
|
}
|
|
|
|
inline Vector3D operator+ (const Vector3D& v1, const Vector3D& v2)
|
|
{
|
|
return Vector3D(v1.x+v2.x,v1.y+v2.y,v1.z+v2.z);
|
|
}
|
|
|
|
inline Vector3D operator- (const Vector3D& v1, const Vector3D& v2)
|
|
{
|
|
return Vector3D(v1.x-v2.x,v1.y-v2.y,v1.z-v2.z);
|
|
}
|
|
|
|
inline float operator* (const Vector3D& v1, const Vector3D& v2)
|
|
{
|
|
return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
|
|
}
|
|
|
|
inline Vector3D CrossProduct(const Vector3D& v1, const Vector3D& v2)
|
|
{
|
|
return Vector3D(v1.y*v2.z-v2.y*v1.z,
|
|
v1.z*v2.x-v2.z*v1.x,
|
|
v1.x*v2.y-v2.x*v1.y);
|
|
}
|
|
|
|
inline Vector3D Transform(const Vector3D& v, const Matrix3D& m)
|
|
{
|
|
return Vector3D(v.x*m._11 + v.y*m._21 + v.z*m._31 + m._41,
|
|
v.x*m._12 + v.y*m._22 + v.z*m._32 + m._42,
|
|
v.x*m._13 + v.y*m._23 + v.z*m._33 + m._43);
|
|
}
|
|
|
|
inline Vector3D rotateVector(const Vector3D& v, const Matrix3D& m)
|
|
{
|
|
return Vector3D(v.x*m._11 + v.y*m._21 + v.z*m._31,
|
|
v.x*m._12 + v.y*m._22 + v.z*m._32,
|
|
v.x*m._13 + v.y*m._23 + v.z*m._33);
|
|
}
|
|
|
|
inline Matrix3D operator*(float scalar, const Matrix3D& m)
|
|
{
|
|
return Matrix3D(scalar*m(0,0),scalar*m(0,1),scalar*m(0,2),scalar*m(0,3),
|
|
scalar*m(1,0),scalar*m(1,1),scalar*m(1,2),scalar*m(1,3),
|
|
scalar*m(2,0),scalar*m(2,1),scalar*m(2,2),scalar*m(2,3),
|
|
scalar*m(3,0),scalar*m(3,1),scalar*m(3,2),scalar*m(3,3));
|
|
}
|
|
|
|
inline Matrix3D operator*(const Matrix3D& m1, const Matrix3D& m2)
|
|
{
|
|
return Matrix3D(
|
|
m1._11*m2._11 + m1._12*m2._21 + m1._13*m2._31 + m1._14*m2._41,
|
|
m1._11*m2._12 + m1._12*m2._22 + m1._13*m2._32 + m1._14*m2._42,
|
|
m1._11*m2._13 + m1._12*m2._23 + m1._13*m2._33 + m1._14*m2._43,
|
|
m1._11*m2._14 + m1._12*m2._24 + m1._13*m2._34 + m1._14*m2._44,
|
|
m1._21*m2._11 + m1._22*m2._21 + m1._23*m2._31 + m1._24*m2._41,
|
|
m1._21*m2._12 + m1._22*m2._22 + m1._23*m2._32 + m1._24*m2._42,
|
|
m1._21*m2._13 + m1._22*m2._23 + m1._23*m2._33 + m1._24*m2._43,
|
|
m1._21*m2._14 + m1._22*m2._24 + m1._23*m2._34 + m1._24*m2._44,
|
|
m1._31*m2._11 + m1._32*m2._21 + m1._33*m2._31 + m1._34*m2._41,
|
|
m1._31*m2._12 + m1._32*m2._22 + m1._33*m2._32 + m1._34*m2._42,
|
|
m1._31*m2._13 + m1._32*m2._23 + m1._33*m2._33 + m1._34*m2._43,
|
|
m1._31*m2._14 + m1._32*m2._24 + m1._33*m2._34 + m1._34*m2._44,
|
|
m1._41*m2._11 + m1._42*m2._21 + m1._43*m2._31 + m1._44*m2._41,
|
|
m1._41*m2._12 + m1._42*m2._22 + m1._43*m2._32 + m1._44*m2._42,
|
|
m1._41*m2._13 + m1._42*m2._23 + m1._43*m2._33 + m1._44*m2._43,
|
|
m1._41*m2._14 + m1._42*m2._24 + m1._43*m2._34 + m1._44*m2._44);
|
|
}
|
|
|
|
inline void
|
|
Matrix3D::rotate(const Vector3D& v)
|
|
{
|
|
if (v.x!=0.0f) *this = PitchMatrix3D(v.x) * (*this);
|
|
if (v.y!=0.0f) *this = YawMatrix3D (v.y) * (*this);
|
|
if (v.z!=0.0f) *this = RollMatrix3D (v.z) * (*this);
|
|
}
|
|
|
|
inline Matrix3D
|
|
TranslateMatrix3D(const Vector3D& v)
|
|
{
|
|
return Matrix3D(1.0f,0.0f,0.0f,0.0f,
|
|
0.0f,1.0f,0.0f,0.0f,
|
|
0.0f,0.0f,1.0f,0.0f,
|
|
v.x, v.y, v.z,1.0f);
|
|
}
|
|
|
|
|
|
inline Matrix3D
|
|
ScaleMatrix3D(const Vector3D& v)
|
|
{
|
|
return Matrix3D( v.x,0.0f,0.0f,0.0f,
|
|
0.0f, v.y,0.0f,0.0f,
|
|
0.0f,0.0f, v.z,0.0f,
|
|
0.0f,0.0f,0.0f,1.0f);
|
|
}
|
|
|
|
|
|
inline Matrix3D
|
|
ScaleMatrix3D(const float s)
|
|
{
|
|
return ScaleMatrix3D(Vector3D(s,s,s));
|
|
}
|
|
|
|
|
|
inline Matrix3D
|
|
PitchMatrix3D(const float c, const float s)
|
|
{
|
|
return Matrix3D(1.0f, 0.0f, 0.0f, 0.0f,
|
|
0.0f, c, -s, 0.0f,
|
|
0.0f, s, c, 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f);
|
|
}
|
|
|
|
|
|
inline Matrix3D
|
|
PitchMatrix3D(const float theta)
|
|
{
|
|
return PitchMatrix3D((float) cos(theta), (float) sin(theta));
|
|
}
|
|
|
|
|
|
inline Matrix3D
|
|
YawMatrix3D(const float c, const float s)
|
|
{
|
|
return Matrix3D( c, 0.0f, s, 0.0f,
|
|
0.0f, 1.0f, 0.0f, 0.0f,
|
|
-s, 0.0f, c, 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f);
|
|
}
|
|
|
|
|
|
inline Matrix3D
|
|
YawMatrix3D(const float theta)
|
|
{
|
|
return YawMatrix3D((float) cos(theta), (float) sin(theta));
|
|
}
|
|
|
|
|
|
inline Matrix3D
|
|
RollMatrix3D(const float c, const float s)
|
|
{
|
|
return Matrix3D(c, -s, 0.0f, 0.0f,
|
|
s, c, 0.0f, 0.0f,
|
|
0.0f, 0.0f, 1.0f, 0.0f,
|
|
0.0f, 0.0f, 0.0f, 1.0f);
|
|
}
|
|
|
|
|
|
inline Matrix3D
|
|
RollMatrix3D(const float theta)
|
|
{
|
|
return RollMatrix3D((float) cos(theta), (float) sin(theta));
|
|
}
|
|
|
|
|
|
template<class T>
|
|
inline T Max(T a, T b)
|
|
{
|
|
return (a>b ? a : b);
|
|
}
|
|
|
|
template<class T>
|
|
inline T Min(T a, T b)
|
|
{
|
|
return (a<b ? a : b);
|
|
}
|
|
|
|
#endif // H_MATH3D
|