1517 lines
35 KiB
C++
1517 lines
35 KiB
C++
/********************************************************************************
|
|
Copyright (C) 2001-2012 Hugh Bailey <obs.jim@gmail.com>
|
|
|
|
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.
|
|
|
|
You should have received a copy of the GNU General Public License
|
|
along with this program; if not, write to the Free Software
|
|
Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA.
|
|
********************************************************************************/
|
|
|
|
|
|
#include "XT.h"
|
|
|
|
|
|
|
|
|
|
|
|
|
|
/*=========================================================
|
|
3-D Vector
|
|
==========================================================*/
|
|
|
|
Vect Vect::GetInterpolationTangent(const Vect &prev, const Vect &next) const
|
|
{
|
|
return (((*this)-prev)+next-(*this))*0.5f;
|
|
}
|
|
|
|
float Vect::DistFromPlane(const Plane& p) const
|
|
{
|
|
return Dot(p.Dir) - p.Dist;
|
|
}
|
|
|
|
|
|
/*=========================================================
|
|
Quaternion
|
|
==========================================================*/
|
|
|
|
Quat::Quat(const Euler &e)
|
|
{
|
|
SetIdentity() *= e;
|
|
}
|
|
|
|
Quat& Quat::operator=(const Euler &e)
|
|
{
|
|
return SetIdentity() *= e;
|
|
}
|
|
|
|
Quat& Quat::operator*=(const Euler &e)
|
|
{
|
|
Vect E;
|
|
mcpy(&E, &e, sizeof(Euler));
|
|
E *= 0.5f;
|
|
|
|
*this *= Quat(0.0f, 0.0f, sin(E.z), cos(E.z));
|
|
*this *= Quat(0.0f, sin(E.y), 0.0f, cos(E.y));
|
|
*this *= Quat(sin(E.x), 0.0f, 0.0f, cos(E.x));
|
|
w = -w;
|
|
|
|
return *this;
|
|
}
|
|
|
|
inline Vect GetQuatVect(const Quat &q)
|
|
{
|
|
Vect v = (Vect&)q;
|
|
v.w = 0.0f;
|
|
return v;
|
|
}
|
|
|
|
Quat& Quat::operator*=(const Quat &q)
|
|
{
|
|
Vect tempAxis = GetQuatVect(*this), qAxis = GetQuatVect(q), axisOut;
|
|
axisOut = ((qAxis * w) + (tempAxis * q.w)) + (tempAxis ^ qAxis);
|
|
axisOut.w = (w * q.w) - (tempAxis | qAxis);
|
|
*this = (Quat&)axisOut;
|
|
|
|
return *this;
|
|
}
|
|
|
|
Quat& Quat::Log()
|
|
{
|
|
float angle = (float)acos(w);
|
|
float sine = (float)sin(angle);
|
|
|
|
if ((fabs(w) < 1.0f) && (fabs(sine) >= EPSILON))
|
|
{
|
|
sine = angle/sine;
|
|
w = 0.0f;
|
|
return *this *= sine;
|
|
}
|
|
else
|
|
w = 0.0f;
|
|
|
|
return *this;
|
|
}
|
|
|
|
Quat& Quat::Exp()
|
|
{
|
|
float length = sqrtf(x*x+y*y+z*z);
|
|
float sine = sinf(length);
|
|
if(length > EPSILON)
|
|
sine /= length;
|
|
else
|
|
sine = 1.0f;
|
|
|
|
*this *= sine;
|
|
w = cosf(length);
|
|
|
|
return *this;
|
|
}
|
|
|
|
|
|
Quat InterpolateQuat(const Quat &from, const Quat &to, float t)
|
|
{
|
|
float dot = from | to;
|
|
float anglef = acosf(dot);
|
|
float sine,sinei,sinet,sineti;
|
|
|
|
if(anglef >= EPSILON)
|
|
{
|
|
sine = sinf(anglef);
|
|
sinei = 1/sine;
|
|
sinet = sinf(anglef*t)*sinei;
|
|
sineti = sinf(anglef*(1.0f-t))*sinei;
|
|
|
|
return Quat((from * sineti) + (to * sinet));
|
|
}
|
|
return Quat(Lerp<Quat>(from, to, t));
|
|
}
|
|
|
|
|
|
Quat CubicInterpolateQuat(const Quat &q0, const Quat &q1, const Quat &m0, const Quat &m1, float t)
|
|
{
|
|
return InterpolateQuat(InterpolateQuat(q0, q1, t), InterpolateQuat(m0, m1, t), 2*(1-t)*t);
|
|
}
|
|
|
|
|
|
|
|
Quat& Quat::MakeFromAxisAngle(const AxisAngle &aa)
|
|
{
|
|
float halfa = aa.w*0.5f;
|
|
float sine = sinf(halfa);
|
|
|
|
x = aa.x * sine;
|
|
y = aa.y * sine;
|
|
z = aa.z * sine;
|
|
w = cosf(halfa);
|
|
|
|
return *this;
|
|
}
|
|
|
|
AxisAngle Quat::GetAxisAngle() const
|
|
{
|
|
AxisAngle newAA(*this);
|
|
return newAA;
|
|
}
|
|
|
|
AxisAngle& AxisAngle::MakeFromQuat(const Quat &q)
|
|
{
|
|
float len,leni;
|
|
|
|
len = (q.x*q.x)+(q.y*q.y)+(q.z*q.z);
|
|
if(!CloseFloat(len, 0.0f))
|
|
{
|
|
leni = (1.0f/(float)sqrt(len));
|
|
x = q.x * leni;
|
|
y = q.y * leni;
|
|
z = q.z * leni;
|
|
w = (float)acos(q.w)*2.0f;
|
|
}
|
|
else
|
|
x = y = z = w = 0.0f;
|
|
|
|
return *this;
|
|
}
|
|
|
|
struct f4x4
|
|
{
|
|
float ptr[4][4];
|
|
};
|
|
|
|
Quat& Quat::CreateFromMatrix(const Matrix &m)
|
|
{
|
|
float Tr = (m.X.x + m.Y.y + m.Z.z);
|
|
int i,j,k;
|
|
|
|
if (Tr > 0.0f)
|
|
{
|
|
float fourD = sqrt(Tr+1.0f);
|
|
w = fourD*0.5f;
|
|
|
|
float invHalf = 0.5f/fourD;
|
|
x = (m.Y.z - m.Z.y)*invHalf;
|
|
y = (m.Z.x - m.X.z)*invHalf;
|
|
z = (m.X.y - m.Y.x)*invHalf;
|
|
}
|
|
else
|
|
{
|
|
if (m.X.x > m.Y.y)
|
|
i = 0;
|
|
else
|
|
i = 1;
|
|
|
|
f4x4 &val = (f4x4&)m;
|
|
|
|
if (m.Z.z > val.ptr[i][i])
|
|
i = 2;
|
|
|
|
j = (i+1)%3;
|
|
k = (i+2)%3;
|
|
|
|
//----------------------------------
|
|
|
|
float fourD = sqrt((val.ptr[i][i] - val.ptr[j][j] - val.ptr[k][k]) + 1.0f);
|
|
|
|
ptr[i] = fourD*0.5f;
|
|
|
|
float invHalf = 0.5f/fourD;
|
|
ptr[j] = (val.ptr[i][j] + val.ptr[j][i])*invHalf;
|
|
ptr[k] = (val.ptr[i][k] + val.ptr[k][i])*invHalf;
|
|
w = (val.ptr[j][k] - val.ptr[k][j])*invHalf;
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
Quat Quat::GetInterpolationTangent(const Quat &prev, const Quat &next) const
|
|
{
|
|
Quat cinv = this->GetInv();
|
|
|
|
return (*this)*(-((cinv*prev).Log() + (cinv*next).Log())/4.0f).Exp();
|
|
}
|
|
|
|
Vect Quat::GetDirectionVector() const
|
|
{
|
|
Matrix m(*this);
|
|
|
|
return m.Z;
|
|
}
|
|
|
|
Quat& Quat::MakeFromDirection(const Vect &dir)
|
|
{
|
|
Vect identDir(0.0f, 0.0f, 1.0f);
|
|
|
|
if(dir.CloseTo(identDir))
|
|
return SetIdentity();
|
|
else
|
|
{
|
|
float cosValue = fabsf(dir.z); //dir.Dot(identDir);
|
|
float angle = acos(cosValue);
|
|
Vect cross = Vect(dir.y, -dir.x, 0.0f).Norm(); //(dir.GetCross(identDir).Norm()
|
|
|
|
return MakeFromAxisAngle(AxisAngle(cross.x, cross.y, cross.z, angle)).Norm();
|
|
}
|
|
}
|
|
|
|
Quat& Quat::SetLookDirection(const Vect &dir)
|
|
{
|
|
Vect newDir = -dir.GetNorm();
|
|
Quat XZrot, YZrot;
|
|
|
|
XZrot.SetIdentity();
|
|
YZrot.SetIdentity();
|
|
|
|
BOOL bXZValid = !CloseFloat(newDir.x, 0.0f, EPSILON) || !CloseFloat(newDir.z, 0.0f, EPSILON);
|
|
BOOL bYZValid = !CloseFloat(newDir.y, 0.0f, EPSILON);
|
|
|
|
if(bXZValid)
|
|
XZrot = AxisAngle(0.0f, 1.0f, 0.0f, atan2f(newDir.x, newDir.z));
|
|
if(bYZValid)
|
|
YZrot = AxisAngle(-1.0f, 0.0f, 0.0f, asinf(newDir.y));
|
|
|
|
if(!bXZValid)
|
|
*this = YZrot;
|
|
else if(!bYZValid)
|
|
*this = XZrot;
|
|
else
|
|
*this = XZrot *= YZrot;
|
|
|
|
return *this;
|
|
}
|
|
|
|
|
|
/*=========================================================
|
|
Bounds
|
|
==========================================================*/
|
|
|
|
Vect Bounds::GetPoint(unsigned int i) const
|
|
{
|
|
assert(i < 8);
|
|
|
|
Vect v;
|
|
|
|
// Note:
|
|
//
|
|
// 0 = min.x,min.y,min.z
|
|
// 1 = min.x,min.y,MAX.z
|
|
// 2 = min.x,MAX.y,min.z
|
|
// 3 = min.x,MAX.y,MAX.z
|
|
// 4 = MAX.x,min.y,min.z
|
|
// 5 = MAX.x,min.y,MAX.z
|
|
|
|
// 6 = MAX.x,MAX.y,min.z
|
|
// 7 = MAX.x,MAX.y,MAX.z
|
|
|
|
if(i > 3) {v.x = Max.x; i -= 4;}
|
|
else {v.x = Min.x;}
|
|
|
|
if(i > 1) {v.y = Max.y; i -= 2;}
|
|
else {v.y = Min.y;}
|
|
|
|
if(i == 1) {v.z = Max.z;}
|
|
else {v.z = Min.z;}
|
|
|
|
return v;
|
|
}
|
|
|
|
Bounds Bounds::GetTransformedBounds(const Matrix &m) const
|
|
{
|
|
Bounds newBounds;
|
|
BOOL bInitialized=0;
|
|
for(int i=0; i<8; i++)
|
|
{
|
|
Vect p = GetPoint(i);
|
|
p.TransformPoint(m);
|
|
|
|
if(!bInitialized)
|
|
{
|
|
newBounds.Max = newBounds.Min = p;
|
|
bInitialized = 1;
|
|
}
|
|
else
|
|
{
|
|
if(p.x < newBounds.Min.x)
|
|
newBounds.Min.x = p.x;
|
|
else if(p.x > newBounds.Max.x)
|
|
newBounds.Max.x = p.x;
|
|
|
|
if(p.y < newBounds.Min.y)
|
|
newBounds.Min.y = p.y;
|
|
else if(p.y > newBounds.Max.y)
|
|
newBounds.Max.y = p.y;
|
|
|
|
if(p.z < newBounds.Min.z)
|
|
newBounds.Min.z = p.z;
|
|
else if(p.z > newBounds.Max.z)
|
|
newBounds.Max.z = p.z;
|
|
}
|
|
}
|
|
|
|
return newBounds;
|
|
}
|
|
|
|
Bounds& Bounds::Transform(const Matrix &m)
|
|
{
|
|
return (*this = GetTransformedBounds(m));
|
|
}
|
|
|
|
Bounds& Bounds::TransformNoRot(const Matrix &m)
|
|
{
|
|
Min.TransformPoint(m);
|
|
Max.TransformPoint(m);
|
|
return *this;
|
|
}
|
|
|
|
BOOL Bounds::IntersectsOBB(const Bounds &test, const Matrix &transform) const
|
|
{
|
|
Bounds testT = test.GetTransformedBounds(transform);
|
|
Bounds thisT = GetTransformedBounds(transform.GetTranspose());
|
|
|
|
if(Intersects(testT) && thisT.Intersects(test))
|
|
return TRUE;
|
|
|
|
return FALSE;
|
|
}
|
|
|
|
int Bounds::PlaneTest(const Plane &p) const
|
|
{
|
|
Vect vmin,vmax;
|
|
|
|
for(int i=0; i<3; i++)
|
|
{
|
|
if(p.Dir.ptr[i] >= 0.0f)
|
|
{
|
|
vmin.ptr[i] = Min.ptr[i];
|
|
vmax.ptr[i] = Max.ptr[i];
|
|
}
|
|
else
|
|
{
|
|
vmin.ptr[i] = Max.ptr[i];
|
|
vmax.ptr[i] = Min.ptr[i];
|
|
}
|
|
}
|
|
|
|
if(vmin.DistFromPlane(p) > 0.0f) return BOUNDS_OUTSIDE;
|
|
|
|
if(vmax.DistFromPlane(p) >= 0.0f) return BOUNDS_PARTIAL;
|
|
|
|
return BOUNDS_INSIDE;
|
|
}
|
|
|
|
BOOL Bounds::OutsidePlane(const Plane &p) const
|
|
{
|
|
Vect vmin;
|
|
|
|
vmin.x = (*(DWORD*)&p.Dir.x & 0x80000000) ? Max.x : Min.x;
|
|
vmin.y = (*(DWORD*)&p.Dir.y & 0x80000000) ? Max.y : Min.y;
|
|
vmin.z = (*(DWORD*)&p.Dir.z & 0x80000000) ? Max.z : Min.z;
|
|
|
|
return (vmin.Dot(p.Dir) > p.Dist);
|
|
}
|
|
|
|
BOOL Bounds::CylinderIntersects(const Vect ¢er, float radius, float height) const
|
|
{
|
|
float radiusAdj = radius+0.01f;
|
|
float heightAdj = height+0.01f;
|
|
|
|
if( (center.x > (Max.x + radiusAdj)) ||
|
|
(center.x < (Min.x - radiusAdj)) ||
|
|
(center.y > (Max.y + heightAdj)) ||
|
|
(center.y < (Min.y - heightAdj)) ||
|
|
(center.z > (Max.z + radiusAdj)) ||
|
|
(center.z < (Min.z - radiusAdj)) )
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
BOOL Bounds::RayIntersection(const Vect &rayOrig, const Vect &rayDir, float &fT) const
|
|
{
|
|
float tMax = M_INFINITE;
|
|
float tMin = -M_INFINITE;
|
|
|
|
Vect center = GetCenter();
|
|
Vect E = Max-center;
|
|
Vect T = center-rayOrig;
|
|
|
|
for(int i=0; i<3; i++)
|
|
{
|
|
float e = T.ptr[i];
|
|
float f = rayDir.ptr[i];
|
|
float fI = 1.0f/f;
|
|
|
|
if(fabs(f) > 0.0f)
|
|
{
|
|
float t1 = (e+E.ptr[i])*fI;
|
|
float t2 = (e-E.ptr[i])*fI;
|
|
if(t1 > t2)
|
|
{
|
|
if(t2 > tMin) tMin = t2;
|
|
if(t1 < tMax) tMax = t1;
|
|
}
|
|
else
|
|
{
|
|
if(t1 > tMin) tMin = t1;
|
|
if(t2 < tMax) tMax = t2;
|
|
}
|
|
if(tMin > tMax) return FALSE;
|
|
if(tMax < 0.0f) return FALSE;
|
|
}
|
|
else if( ((-e - E.ptr[i]) > 0.0f) ||
|
|
((-e + E.ptr[i]) < 0.0f) )
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
|
|
if(tMin > 0.0f)
|
|
{
|
|
fT = tMin;
|
|
return TRUE;
|
|
}
|
|
else
|
|
{
|
|
fT = tMax;
|
|
return TRUE;
|
|
}
|
|
}
|
|
|
|
BOOL Bounds::RayIntersectionTopDown(const Vect &rayOrig, const Vect &rayDir, float &fT) const
|
|
{
|
|
float tMax = M_INFINITE;
|
|
float tMin = -M_INFINITE;
|
|
|
|
Vect2 newRayDir(rayDir.x, rayDir.y);
|
|
newRayDir.Norm();
|
|
|
|
Vect2 center(((Max.x-Min.x)*0.5f)+Min.x, ((Max.z-Min.z)*0.5f)+Min.z);
|
|
Vect2 E(Max.x-center.x, Max.z-center.y);
|
|
Vect2 T(center.x-rayOrig.x, center.y-rayOrig.z);
|
|
|
|
for(int i=0; i<2; i++)
|
|
{
|
|
float e = T.ptr[i];
|
|
float f = newRayDir.ptr[i];
|
|
float fI = 1.0f/f;
|
|
|
|
if(fabs(f) > 0.0f)
|
|
{
|
|
float t1 = (e+E.ptr[i])*fI;
|
|
float t2 = (e-E.ptr[i])*fI;
|
|
if(t1 > t2)
|
|
{
|
|
if(t2 > tMin) tMin = t2;
|
|
if(t1 < tMax) tMax = t1;
|
|
}
|
|
else
|
|
{
|
|
if(t1 > tMin) tMin = t1;
|
|
if(t2 < tMax) tMax = t2;
|
|
}
|
|
if(tMin > tMax) return FALSE;
|
|
if(tMax < 0.0f) return FALSE;
|
|
}
|
|
else if( ((-e - E.ptr[i]) > 0.0f) ||
|
|
((-e + E.ptr[i]) < 0.0f) )
|
|
{
|
|
return FALSE;
|
|
}
|
|
}
|
|
|
|
if(tMin > 0.0f)
|
|
{
|
|
fT = tMin;
|
|
return TRUE;
|
|
}
|
|
else
|
|
{
|
|
fT = tMax;
|
|
return TRUE;
|
|
}
|
|
}
|
|
|
|
BOOL Bounds::LineIntersection(const Vect &v1, const Vect &v2, float &fT) const
|
|
{
|
|
float tMax = M_INFINITE;
|
|
float tMin = -M_INFINITE;
|
|
|
|
Vect rayVect = (v2-v1);
|
|
float rayLength = rayVect.Len();
|
|
Vect rayDir = rayVect*(1.0f/rayLength);
|
|
|
|
Vect center = GetCenter();
|
|
Vect E = Max-center;
|
|
Vect T = center-v1;
|
|
|
|
for(int i=0; i<3; i++)
|
|
{
|
|
float e = T.ptr[i];
|
|
float f = rayDir.ptr[i];
|
|
float fI = 1.0f/f;
|
|
|
|
if(fabs(f) > 0.0f)
|
|
{
|
|
float t1 = (e+E.ptr[i])*fI;
|
|
float t2 = (e-E.ptr[i])*fI;
|
|
if(t1 > t2)
|
|
{
|
|
if(t2 > tMin) tMin = t2;
|
|
if(t1 < tMax) tMax = t1;
|
|
}
|
|
else
|
|
{
|
|
if(t1 > tMin) tMin = t1;
|
|
if(t2 < tMax) tMax = t2;
|
|
}
|
|
if(tMin > tMax) return FALSE;
|
|
if(tMax < 0.0f) return FALSE;
|
|
}
|
|
else if( ((-e - E.ptr[i]) > 0.0f) ||
|
|
((-e + E.ptr[i]) < 0.0f) )
|
|
{
|
|
return FALSE;
|
|
}
|
|
|
|
if(tMin > rayLength) return FALSE;
|
|
}
|
|
|
|
if(tMin > 0.0f)
|
|
{
|
|
fT = (tMin/rayLength);
|
|
return TRUE;
|
|
}
|
|
else
|
|
{
|
|
fT = (tMax/rayLength);
|
|
return TRUE;
|
|
}
|
|
}
|
|
|
|
BOOL Bounds::LineIntersectionTopDown(const Vect &v1, const Vect &v2, float &fT) const
|
|
{
|
|
float tMax = M_INFINITE;
|
|
float tMin = -M_INFINITE;
|
|
|
|
Vect2 rayVect(v2.x-v1.x, v2.z-v1.z);
|
|
float rayLength = rayVect.Len();
|
|
Vect2 rayDir = rayVect*(1.0f/rayLength);
|
|
|
|
Vect2 center(((Max.x-Min.x)*0.5f)+Min.x, ((Max.z-Min.z)*0.5f)+Min.z);
|
|
Vect2 E(Max.x-center.x, Max.z-center.y);
|
|
Vect2 T(center.x-v1.x, center.y-v1.z);
|
|
|
|
for(int i=0; i<2; i++)
|
|
{
|
|
float e = T.ptr[i];
|
|
float f = rayDir.ptr[i];
|
|
float fI = 1.0f/f;
|
|
|
|
if(fabs(f) > 0.0f)
|
|
{
|
|
float t1 = (e+E.ptr[i])*fI;
|
|
float t2 = (e-E.ptr[i])*fI;
|
|
if(t1 > t2)
|
|
{
|
|
if(t2 > tMin) tMin = t2;
|
|
if(t1 < tMax) tMax = t1;
|
|
}
|
|
else
|
|
{
|
|
if(t1 > tMin) tMin = t1;
|
|
if(t2 < tMax) tMax = t2;
|
|
}
|
|
if(tMin > tMax) return FALSE;
|
|
if(tMax < 0.0f) return FALSE;
|
|
}
|
|
else if( ((-e - E.ptr[i]) > 0.0f) ||
|
|
((-e + E.ptr[i]) < 0.0f) )
|
|
{
|
|
return FALSE;
|
|
}
|
|
|
|
if(tMin > rayLength) return FALSE;
|
|
}
|
|
|
|
if(tMin > 0.0f)
|
|
{
|
|
fT = (tMin/rayLength);
|
|
return TRUE;
|
|
}
|
|
else
|
|
{
|
|
fT = (tMax/rayLength);
|
|
return TRUE;
|
|
}
|
|
}
|
|
|
|
|
|
float Bounds::MinDistFrom(const Plane &p) const
|
|
{
|
|
float vecLen = VectorOffsetLength(p.Dir)*0.5f;
|
|
float centerDist = GetCenter().DistFromPlane(p);
|
|
|
|
return (p.Dist+centerDist)-vecLen;
|
|
}
|
|
|
|
Bounds& Bounds::MakeFromPoints(const List<Vect> &pointList)
|
|
{
|
|
if(!pointList.Num()) return *this;
|
|
|
|
Min = pointList[0];
|
|
Max = pointList[0];
|
|
for(unsigned int i=1; i<pointList.Num(); i++)
|
|
{
|
|
Min.ClampMax(pointList[i]);
|
|
Max.ClampMin(pointList[i]);
|
|
}
|
|
|
|
return *this;
|
|
}
|
|
|
|
|
|
/*=========================================================
|
|
Plane
|
|
==========================================================*/
|
|
|
|
Plane& Plane::Transform(const Matrix &m)
|
|
{
|
|
Dir.TransformVector(m).Norm();
|
|
Dist -= Dir.Dot(m.T.GetTransformedVector(m));
|
|
return *this;
|
|
}
|
|
|
|
void Plane::CreateFromTri(const Vect &a, const Vect &b, const Vect &c)
|
|
{
|
|
Dir = ((b-a)^(c-a)).Norm();
|
|
Dist = (a | Dir);
|
|
}
|
|
|
|
BOOL Plane::GetRayIntersection(const Vect &rayOrigin, const Vect &rayDir, float &fT) const
|
|
{
|
|
float c0 = Dir | rayDir;
|
|
|
|
fT = 0;
|
|
|
|
if(fabs(c0) < EPSILON)
|
|
return 0;
|
|
|
|
fT = ((Dist - (Dir | rayOrigin)) / c0);
|
|
|
|
return 1;
|
|
}
|
|
|
|
BOOL Plane::GetIntersection(const Vect &p1, const Vect &p2, float &fT) const
|
|
{
|
|
float P1Dist = p1.DistFromPlane(*this);
|
|
float P2Dist = p2.DistFromPlane(*this);
|
|
|
|
if(CloseFloat(P1Dist, 0.0f, EPSILON))
|
|
{
|
|
if(P2Dist == 0.0f)
|
|
return 0;
|
|
|
|
fT = 0.0f;
|
|
return 1;
|
|
}
|
|
else if(CloseFloat(P2Dist, 0.0f, EPSILON))
|
|
{
|
|
fT = 1.0f;
|
|
return 1;
|
|
}
|
|
|
|
BOOL bP1Over = (P1Dist > 0.0f);
|
|
BOOL bP2Over = (P2Dist > 0.0f);
|
|
|
|
if(bP1Over == bP2Over)
|
|
return FALSE;
|
|
|
|
float P1AbsDist = fabs(P1Dist);
|
|
float dist2 = (P1AbsDist+fabs(P2Dist));
|
|
if(dist2 < EPSILON)
|
|
return FALSE;
|
|
fT = P1AbsDist/dist2;
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
BOOL Plane::GetDoublePlaneIntersection(const Plane &p2, Vect &intOrigin, Vect &intDir) const
|
|
{
|
|
float fCosAngle = (Dir|p2.Dir);
|
|
float fSinAngle = sqrt(1.0f-(fCosAngle*fCosAngle));
|
|
|
|
if(fabs(fSinAngle) < LARGE_EPSILON)
|
|
return FALSE;
|
|
|
|
float fInvSinAngle = 1.0f/fSinAngle;
|
|
|
|
float fChi1 = (Dist-(fCosAngle*p2.Dist))*fInvSinAngle;
|
|
float fChi2 = (p2.Dist-(fCosAngle*Dist))*fInvSinAngle;
|
|
|
|
intDir = Dir^p2.Dir;
|
|
float iLen = 1.0f/intDir.Len();
|
|
intDir *= iLen;
|
|
|
|
intOrigin = (Dir*fChi1) + (p2.Dir*fChi2);
|
|
intOrigin *= iLen;
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
BOOL Plane::GetTriplePlaneIntersection(const Plane &p2, const Plane &p3, Vect &intersection) const
|
|
{
|
|
Vect rayOrigin, rayDir;
|
|
|
|
if(GetDoublePlaneIntersection(p2, rayOrigin, rayDir))
|
|
{
|
|
float fT;
|
|
if(p3.GetRayIntersection(rayOrigin, rayDir, fT))
|
|
{
|
|
intersection = rayOrigin + (rayDir*fT);
|
|
return TRUE;
|
|
}
|
|
}
|
|
|
|
return FALSE;
|
|
}
|
|
|
|
/*=========================================================
|
|
Matrix (3x4)
|
|
==========================================================*/
|
|
|
|
void Matrix::CreateFromQuat(const Quat &q)
|
|
{
|
|
float norm = q | q;
|
|
float s = (norm > 0) ? 2/norm : 0;
|
|
|
|
float xx = q.x * q.x * s;
|
|
float yy = q.y * q.y * s;
|
|
float zz = q.z * q.z * s;
|
|
float xy = q.x * q.y * s;
|
|
float xz = q.x * q.z * s;
|
|
float yz = q.y * q.z * s;
|
|
float wx = q.w * q.x * s;
|
|
float wy = q.w * q.y * s;
|
|
float wz = q.w * q.z * s;
|
|
|
|
|
|
X.x = 1.0f - (yy + zz);
|
|
X.y = xy + wz;
|
|
X.z = xz - wy;
|
|
|
|
Y.x = xy - wz;
|
|
Y.y = 1.0f - (xx + zz);
|
|
Y.z = yz + wx;
|
|
|
|
Z.x = xz + wy;
|
|
Z.y = yz - wx;
|
|
Z.z = 1.0f - (xx + yy);
|
|
}
|
|
|
|
|
|
Matrix Matrix::GetInverse() const
|
|
{
|
|
return Matrix(*this).Inverse();
|
|
}
|
|
|
|
Matrix& Matrix::Inverse()
|
|
{
|
|
float m[16], cur[16];
|
|
|
|
Matrix4x4Convert(cur, *this);
|
|
Matrix4x4Inverse(m, cur);
|
|
Matrix4x4Convert(*this, m);
|
|
|
|
return *this;
|
|
}
|
|
|
|
/*=========================================================
|
|
Line2
|
|
==========================================================*/
|
|
|
|
BOOL Line2::LinesIntersect(const Line2 &collider) const
|
|
{
|
|
//this looks more complex than it really is. I'm just an optimization freak.
|
|
//...okay, this probably isn't the kind of function that's called every frame or something.
|
|
//but I'm compulsive about optimization! ..okay, I'll make it cleaner one of these days.
|
|
|
|
Vect2 vec = (A-B);
|
|
float len = vec.Len();
|
|
Vect2 norm = (vec/len);
|
|
Vect2 cross = norm.GetCross();
|
|
float dist = cross.Dot(A);
|
|
|
|
float fADist = collider.A.Dot(cross)-dist;
|
|
float fBDist = collider.B.Dot(cross)-dist;
|
|
|
|
BOOL bAAbove = (fADist > -EPSILON);
|
|
BOOL bBAbove = (fBDist > -EPSILON);
|
|
|
|
BOOL bFoundZeroLine1 = FALSE;
|
|
|
|
if( (fabs(fADist) < EPSILON) ||
|
|
(fabs(fBDist) < EPSILON) )
|
|
{
|
|
bFoundZeroLine1 = TRUE;
|
|
}
|
|
|
|
if(bAAbove == bBAbove)
|
|
return FALSE;
|
|
|
|
vec = (collider.A-collider.B);
|
|
len = vec.Len();
|
|
norm = (vec/len);
|
|
cross = norm.GetCross();
|
|
dist = cross.Dot(collider.A);
|
|
|
|
fADist = A.Dot(cross)-dist;
|
|
fBDist = B.Dot(cross)-dist;
|
|
|
|
bAAbove = (fADist > -EPSILON);
|
|
bBAbove = (fBDist > -EPSILON);
|
|
|
|
if(fabs(fADist) < EPSILON)
|
|
bAAbove = bBAbove;
|
|
if(fabs(fBDist) < EPSILON)
|
|
bBAbove = bAAbove;
|
|
|
|
BOOL bFoundZeroLine2 = FALSE;
|
|
|
|
if( (fabs(fADist) < EPSILON) ||
|
|
(fabs(fBDist) < EPSILON) )
|
|
{
|
|
bFoundZeroLine2 = TRUE;
|
|
}
|
|
|
|
if(bAAbove == bBAbove)
|
|
return FALSE;
|
|
|
|
if(bFoundZeroLine1 && bFoundZeroLine2)
|
|
return FALSE;
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
|
|
/*=========================================================
|
|
Other
|
|
==========================================================*/
|
|
|
|
Vect GetHSpline(const Vect &v0, const Vect &v1, const Vect &m0, const Vect &m1, float fT)
|
|
{
|
|
float fT2 = fT*fT;
|
|
float fT3 = fT2*fT;
|
|
|
|
float fT2_2 = fT2*2;
|
|
float fT2_3 = fT2*3;
|
|
float fT3_2 = fT3*2;
|
|
|
|
return Vect((v0*(fT3_2-fT2_3+1)) + (m0*(fT3-fT2_2+fT)) + (m1*(fT3-fT2)) + (v1*(-fT3_2 + fT2_3)));
|
|
}
|
|
|
|
BOOL SphereRayCollision(const Vect ¢er, float radius, const Vect &rayOrig, const Vect &rayDir, Vect *collision, Plane *collisionPlane)
|
|
{
|
|
//the standard algorithm
|
|
Vect l = (center-rayOrig);
|
|
float d = l | rayDir; //distance from rayDir Plane
|
|
|
|
if(d < radius)
|
|
return FALSE;
|
|
|
|
float l2 = l | l; //c-o distance squared
|
|
float r2 = radius*radius;
|
|
|
|
if(l2 < r2) //if inside the sphere, fail
|
|
return FALSE;
|
|
|
|
if((d < 0.0f) && (l2 > r2)) //if the plane distance is negative, and
|
|
//the distance is over the radius, fail
|
|
return FALSE;
|
|
|
|
float m2 = l2 - (d*d); //distance from the sphere center to the
|
|
//closest ray point
|
|
|
|
if(m2 > r2) return FALSE; //if m2 is larger than the radius, fail
|
|
|
|
float q = sqrt(r2-m2); //real distance from the edge of the
|
|
//sphere to the cloest ray point
|
|
//(forms a sort of triangle)
|
|
float fT;
|
|
|
|
fT = (l2 > r2) ? (d-q) : (d+q); //if the distance is over the radius,
|
|
//d-q = T, else d+q=T
|
|
|
|
Vect collisionValue = rayOrig+(rayDir*fT);
|
|
|
|
if(collision)
|
|
*collision = collisionValue;
|
|
|
|
if(collisionPlane)
|
|
{
|
|
collisionPlane->Dir = (collisionValue-center).Norm();
|
|
collisionPlane->Dist = collisionPlane->Dir|collisionValue;
|
|
}
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
// yes, I realize this breaks on caps when the ray origin is inside the cylinder.
|
|
// it's not like it's actually going to be in there anyway.
|
|
BOOL CylinderRayCollision(const Vect ¢er, float radius, float height, const Vect &rayOrig, const Vect &rayDir, Vect *collision, Plane *collisionPlane)
|
|
{
|
|
Vect collisionValue;
|
|
BOOL bHit = FALSE;
|
|
float fT;
|
|
|
|
Plane axisPlane(0.0f, 1.0f, 0.0f, center.y);
|
|
|
|
//---------------------------------------
|
|
// test the cap
|
|
if(fabs(rayDir.y) > EPSILON)
|
|
{
|
|
BOOL bUnder = (rayDir.y<0.0f);
|
|
|
|
Plane planeCap;
|
|
planeCap.Dir.Set(0.0f, bUnder ? 1.0f : -1.0f, 0.0f);
|
|
planeCap.Dist = (bUnder ? center.y : -center.y)+height;
|
|
|
|
if(rayOrig.DistFromPlane(planeCap) > 0.0f)
|
|
{
|
|
if(planeCap.GetRayIntersection(rayOrig, rayDir, fT))
|
|
{
|
|
collisionValue = rayOrig+(rayDir*fT);
|
|
Vect CapCenter = center+(planeCap.Dir*height);
|
|
|
|
if(collisionValue.Dist(CapCenter) <= radius)
|
|
{
|
|
if(collisionPlane)
|
|
*collisionPlane = planeCap;
|
|
bHit = TRUE;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
if(!bHit && ((1.0f-fabs(rayDir.y)) > EPSILON))
|
|
{
|
|
//---------------------------------------
|
|
// test the body
|
|
Vect adjDir, adjCenter;
|
|
|
|
adjDir.Set(rayDir.x, 0.0f, rayDir.z).Norm();
|
|
adjCenter.Set(center.x, rayOrig.y, center.z);
|
|
|
|
Vect l = (adjCenter-rayOrig);
|
|
float d = l | adjDir; //distance from adjDir Plane
|
|
float l2 = l | l; //c-o distance squared
|
|
float r2 = radius*radius;
|
|
|
|
if(l2 < r2) //if inside the cylinder, fail
|
|
return FALSE;
|
|
|
|
if((d < 0.0f) && (l2 > r2)) //if the plane distance is negative, and
|
|
return FALSE; //the distance is over the radius, fail
|
|
|
|
float m2 = l2 - (d*d); //distance from the cylinder center to
|
|
//the closest ray point
|
|
|
|
if(m2 > r2) //if m2 is larger than the radius, fail
|
|
return FALSE;
|
|
|
|
float q = sqrt(r2-m2); //real distance from the edge of the
|
|
//cylinder to the cloest ray point
|
|
//(forms a sort of triangle)
|
|
|
|
fT = (l2 > r2) ? (d-q) : (d+q); //if the distance is over the radius,
|
|
//d-q = T, else d+q=T
|
|
//distance
|
|
|
|
fT /= (adjDir|rayDir); //divide by angle to get the proper
|
|
//value
|
|
|
|
collisionValue = rayOrig+(rayDir*fT);
|
|
|
|
if(fabs(collisionValue.DistFromPlane(axisPlane)) >= height)
|
|
return FALSE;
|
|
|
|
if(collisionPlane)
|
|
{
|
|
Vect temp = collisionValue;
|
|
temp.y = center.y;
|
|
|
|
collisionPlane->Dir = (temp-center).Norm();
|
|
collisionPlane->Dist = collisionPlane->Dir|temp;
|
|
}
|
|
|
|
bHit = TRUE;
|
|
}
|
|
|
|
if(!bHit)
|
|
return FALSE;
|
|
|
|
if(collision)
|
|
*collision = collisionValue;
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
BOOL PointOnFiniteLine(const Vect &lineV1, const Vect &lineV2, const Vect &p)
|
|
{
|
|
Vect line = lineV2-lineV1;
|
|
float lineDist = line.Len();
|
|
|
|
Plane plane;
|
|
plane.Dir = line*(1.0f/lineDist);
|
|
plane.Dist = plane.Dir.Dot(lineV1);
|
|
|
|
float dist = p.DistFromPlane(plane);
|
|
if((dist < 0.0f) || (dist > lineDist))
|
|
return FALSE;
|
|
|
|
float fT = (dist/lineDist);
|
|
|
|
return p.CloseTo(Lerp(lineV1, lineV2, fT));
|
|
}
|
|
|
|
|
|
BOOL ClosestLinePoint(const Vect &ray1Origin, const Vect &ray1Dir, const Vect &ray2Origin, const Vect &ray2Dir, float &fT)
|
|
{
|
|
Vect W = (ray1Origin-ray2Origin);
|
|
|
|
float a = ray1Dir|ray1Dir;
|
|
float b = ray1Dir|ray2Dir;
|
|
float c = ray2Dir|ray2Dir;
|
|
float d = ray1Dir|W;
|
|
float e = ray2Dir|W;
|
|
|
|
float c0 = (a*c)-(b*b);
|
|
|
|
if(c0 < EPSILON)
|
|
return FALSE;
|
|
|
|
float c1 = (b*e)-(c*d);
|
|
|
|
fT = c1/c0;
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
|
|
BOOL ClosestLinePoints(const Vect &ray1Origin, const Vect &ray1Dir, const Vect &ray2Origin, const Vect &ray2Dir, float &fT1, float &fT2)
|
|
{
|
|
Vect W = (ray1Origin-ray2Origin);
|
|
|
|
float a = ray1Dir|ray1Dir;
|
|
float b = ray1Dir|ray2Dir;
|
|
float c = ray2Dir|ray2Dir;
|
|
float d = ray1Dir|W;
|
|
float e = ray2Dir|W;
|
|
|
|
float c0 = (a*c)-(b*b);
|
|
|
|
if(c0 < EPSILON)
|
|
return FALSE;
|
|
|
|
float c1 = (b*e)-(c*d);
|
|
float c2 = (a*e)-(b*d);
|
|
|
|
fT1 = c1/c0;
|
|
fT2 = c2/c0;
|
|
|
|
return TRUE;
|
|
}
|
|
|
|
|
|
Vect CartToPolar(const Vect &cart)
|
|
{
|
|
Vect polar;
|
|
|
|
polar.z = cart.Len();
|
|
|
|
if(CloseFloat(polar.z, 0.0f, EPSILON))
|
|
{
|
|
polar.x = 0.0f;
|
|
polar.y = 0.0f;
|
|
polar.z = 0.0f;
|
|
}
|
|
else
|
|
{
|
|
polar.x = asin(cart.y / polar.z);
|
|
polar.y = atan2(cart.x, cart.z);
|
|
}
|
|
|
|
return polar;
|
|
}
|
|
|
|
Vect PolarToCart(const Vect &polar)
|
|
{
|
|
Vect cart;
|
|
|
|
float sinx = cos(polar.x);
|
|
|
|
cart.x = polar.z * sinx * sin(polar.y);
|
|
cart.z = polar.z * sinx * cos(polar.y);
|
|
cart.y = polar.z * sin(polar.x);
|
|
|
|
return cart;
|
|
}
|
|
|
|
Vect PolarToCart(float latitude, float longitude, float dist)
|
|
{
|
|
Vect cart;
|
|
|
|
float sinx = cos(latitude);
|
|
|
|
cart.x = dist * sinx * sin(longitude);
|
|
cart.z = dist * sinx * cos(longitude);
|
|
cart.y = dist * sin(latitude);
|
|
|
|
return cart;
|
|
}
|
|
|
|
|
|
Vect2 NormToPolar(const Vect &norm)
|
|
{
|
|
Vect2 polar;
|
|
|
|
polar.x = atan2(norm.x, norm.z);
|
|
polar.y = asin(norm.y);
|
|
|
|
return polar;
|
|
}
|
|
|
|
void NormToPolar(const Vect &norm, float &latitude, float &longitude)
|
|
{
|
|
longitude = atan2(norm.x, norm.z);
|
|
latitude = acos(norm.y);
|
|
}
|
|
|
|
Vect PolarToNorm(const Vect2 &polar)
|
|
{
|
|
Vect norm;
|
|
|
|
float sinx = sin(polar.x);
|
|
|
|
norm.x = sinx * cos(polar.y);
|
|
norm.y = sinx * sin(polar.y);
|
|
norm.z = cos(polar.x);
|
|
|
|
return norm;
|
|
}
|
|
|
|
|
|
float GetPlaneCylinderOffset(const Vect &dir, float cylHalfHeight, float cylRadius)
|
|
{
|
|
Vect2 dir2(sqrtf(1.0f-(dir.y*dir.y)), fabs(dir.y));
|
|
Vect2 centerOffset(cylRadius, cylHalfHeight);
|
|
|
|
return centerOffset.Dot(dir2);
|
|
}
|
|
|
|
float GetPlaneCapsuleOffset(const Vect &dir, float capsuleHalfHeight, float capsuleRadius)
|
|
{
|
|
Vect2 dir2(sqrtf(1.0f-(dir.y*dir.y)), fabs(dir.y));
|
|
Vect2 centerOffset(capsuleRadius, capsuleHalfHeight);
|
|
|
|
return centerOffset.Dot(dir2)-(1.0f-MAX(dir2.x, dir2.y));
|
|
}
|
|
|
|
float GetPlaneConeOffset(const Vect &dir, float coneHeight, float coneRadius)
|
|
{
|
|
Vect2 dir2(sqrtf(1.0f-(dir.y*dir.y)), dir.y);
|
|
Vect2 p1(coneRadius, coneHeight), p2(coneRadius, 0.0f);
|
|
|
|
float dist1 = dir2.Dot(p1);
|
|
float dist2 = dir2.Dot(p2);
|
|
|
|
return MAX(dist1, dist2);
|
|
}
|
|
|
|
|
|
float RandomFloat(BOOL bPositiveOnly)
|
|
{
|
|
if(bPositiveOnly)
|
|
return (float)(((double)rand())/32767.0);
|
|
else
|
|
return (float)(((((double)rand())/32767.0)*2.0)-1.0);
|
|
}
|
|
|
|
Vect RandomVect(BOOL bPositiveOnly)
|
|
{
|
|
return Vect(RandomFloat(bPositiveOnly), RandomFloat(bPositiveOnly), RandomFloat(bPositiveOnly)).Norm();
|
|
}
|
|
|
|
|
|
void CalcTorque(float &val1, float val2, float torque, float minAdjust, float fT)
|
|
{
|
|
if(val1 == val2)
|
|
return;
|
|
|
|
float dist = (val2-val1)*torque; //uses distance to determine torque speed
|
|
|
|
BOOL bOver = dist > 0.0f;
|
|
|
|
if(bOver)
|
|
{
|
|
if(dist < minAdjust) dist = minAdjust; //prevents torque from going too slow
|
|
val1 += dist*fT; //adds toque to val1
|
|
if(val1 > val2) val1 = val2; //sets val1 to val2 if overshot with the minimum torque value
|
|
}
|
|
else
|
|
{
|
|
if(dist > -minAdjust) dist = -minAdjust;
|
|
val1 += dist*fT;
|
|
if(val1 < val2) val1 = val2;
|
|
}
|
|
}
|
|
|
|
//vector version
|
|
void CalcTorque(Vect &val1, const Vect &val2, float torque, float minAdjust, float fT)
|
|
{
|
|
if(val1 == val2)
|
|
return;
|
|
|
|
Vect line = (val2-val1);
|
|
float origDist = line.Len();
|
|
Vect dir = line * (1.0f/origDist);
|
|
|
|
float torqueDist = origDist*torque; //uses distance to determine torque speed
|
|
|
|
float adjustDist = torqueDist*fT;
|
|
|
|
if(torqueDist < minAdjust) torqueDist = minAdjust; //prevents torque from going too slow
|
|
|
|
if(adjustDist <= (origDist-LARGE_EPSILON))
|
|
val1 += dir*adjustDist; //adds toque to val1
|
|
else
|
|
val1 = val2; //sets val1 to val2 if overshot with the minimum torque value
|
|
}
|
|
|
|
|
|
void Matrix4x4Identity(float *destMatrix)
|
|
{
|
|
float matrixOut[4][4];
|
|
|
|
zero(matrixOut, 64);
|
|
|
|
matrixOut[0][0] = matrixOut[1][1] = matrixOut[2][2] = matrixOut[3][3] = 1.0f;
|
|
|
|
mcpy(destMatrix, matrixOut, 64);
|
|
}
|
|
|
|
void Matrix4x4Convert(float *destMatrix, const Matrix &mat)
|
|
{
|
|
mcpy(destMatrix, &mat, sizeof(Matrix));
|
|
|
|
destMatrix[15] = 1.0f;
|
|
}
|
|
|
|
void Matrix4x4Convert(Matrix &mat, float *m4x4)
|
|
{
|
|
mcpy(&mat, m4x4, sizeof(Matrix));
|
|
}
|
|
|
|
|
|
void Matrix4x4Multiply(float *destMatrix, float *M1, float *M2)
|
|
{
|
|
assert(M1 && M2);
|
|
|
|
int i,j;
|
|
float matrixOut[4][4];
|
|
|
|
for(i=0; i<4; i++)
|
|
{
|
|
int pos = i*4;
|
|
Vect4 row(M1[pos], M1[pos+1], M1[pos+2], M1[pos+3]);
|
|
|
|
for(j=0; j<4; j++)
|
|
{
|
|
Vect4 column(M2[j], M2[j+4], M2[j+8], M2[j+12]);
|
|
|
|
matrixOut[i][j] = row | column;
|
|
}
|
|
}
|
|
|
|
mcpy(destMatrix, matrixOut, 64);
|
|
}
|
|
|
|
void Matrix4x4TransformVect(Vect4 &out, float *M1, const Vect4 &vect)
|
|
{
|
|
int i;
|
|
Vect4 vectOut;
|
|
|
|
for(i=0; i<4; i++)
|
|
{
|
|
int pos = i*4;
|
|
Vect4 row(M1[pos], M1[pos+1], M1[pos+2], M1[pos+3]);
|
|
|
|
vectOut.ptr[i] = row|vect;
|
|
}
|
|
|
|
mcpy(out.ptr, vectOut.ptr, 16);
|
|
}
|
|
|
|
void Matrix4x4Transpose(float *destMatrix, float *srcMatrix)
|
|
{
|
|
float matrixOut[4][4];
|
|
|
|
matrixOut[0][0] = srcMatrix[0];
|
|
matrixOut[1][0] = srcMatrix[1];
|
|
matrixOut[2][0] = srcMatrix[2];
|
|
matrixOut[3][0] = srcMatrix[3];
|
|
matrixOut[0][1] = srcMatrix[4];
|
|
matrixOut[1][1] = srcMatrix[5];
|
|
matrixOut[2][1] = srcMatrix[6];
|
|
matrixOut[3][1] = srcMatrix[7];
|
|
matrixOut[0][2] = srcMatrix[8];
|
|
matrixOut[1][2] = srcMatrix[9];
|
|
matrixOut[2][2] = srcMatrix[10];
|
|
matrixOut[3][2] = srcMatrix[11];
|
|
matrixOut[0][3] = srcMatrix[12];
|
|
matrixOut[1][3] = srcMatrix[13];
|
|
matrixOut[2][3] = srcMatrix[14];
|
|
matrixOut[3][3] = srcMatrix[15];
|
|
|
|
mcpy(destMatrix, matrixOut, 64);
|
|
}
|
|
|
|
float Matrix3x3Determinant(float *M1)
|
|
{
|
|
float det;
|
|
|
|
det = (M1[0] * ((M1[4]*M1[8]) - (M1[7]*M1[5])))
|
|
- (M1[1] * ((M1[3]*M1[8]) - (M1[6]*M1[5])))
|
|
+ (M1[2] * ((M1[3]*M1[7]) - (M1[6]*M1[4])));
|
|
|
|
return det;
|
|
}
|
|
|
|
|
|
float Matrix4x4Determinant(float *M1)
|
|
{
|
|
float det, result = 0.0f, i = 1.0f;
|
|
float msub3[9];
|
|
int n;
|
|
|
|
for(n = 0; n < 4; n++, i *= -1.0f)
|
|
{
|
|
Matrix4x4SubMatrix(msub3, M1, 0, n);
|
|
|
|
det = Matrix3x3Determinant(msub3);
|
|
result += (M1[n] * det * i);
|
|
}
|
|
|
|
return result;
|
|
}
|
|
|
|
void Matrix4x4SubMatrix(float *destMatrix, float *M1, int i, int j)
|
|
{
|
|
int ti, tj, idst, jdst;
|
|
|
|
for (ti = 0; ti < 4; ti++)
|
|
{
|
|
if(ti < i)
|
|
idst = ti;
|
|
else if(ti > i)
|
|
idst = ti-1;
|
|
|
|
for(tj = 0; tj < 4; tj++)
|
|
{
|
|
if(tj < j)
|
|
jdst = tj;
|
|
else if(tj > j)
|
|
jdst = tj-1;
|
|
|
|
if((ti != i) && (tj != j))
|
|
destMatrix[(idst*3) + jdst] = M1[(ti*4) + tj];
|
|
}
|
|
}
|
|
}
|
|
|
|
BOOL Matrix4x4Inverse(float *destMatrix, float *M1)
|
|
{
|
|
float mdet = Matrix4x4Determinant(M1);
|
|
float mtemp[9];
|
|
int i, j, sign;
|
|
|
|
if(fabs(mdet) < 0.0005f)
|
|
return 0;
|
|
|
|
for (i = 0; i < 4; i++)
|
|
{
|
|
for (j = 0; j < 4; j++)
|
|
{
|
|
sign = 1 - ((i+j) % 2) * 2;
|
|
|
|
Matrix4x4SubMatrix(mtemp, M1, i, j);
|
|
|
|
destMatrix[i+(j*4)] = (Matrix3x3Determinant(mtemp) * sign) / mdet;
|
|
}
|
|
}
|
|
|
|
return 1;
|
|
}
|
|
|
|
void Matrix4x4Ortho(float *destMatrix, double left, double right, double bottom, double top, double near, double far)
|
|
{
|
|
float matrixOut[4][4];
|
|
zero(matrixOut, 64);
|
|
|
|
matrixOut[0][0] = float(2.0 / (right-left));
|
|
matrixOut[3][0] = float((left+right) / (left-right));
|
|
|
|
matrixOut[1][1] = float(2.0 / (top-bottom));
|
|
matrixOut[3][1] = float((bottom+top) / (bottom-top));
|
|
|
|
matrixOut[2][2] = float(1.0 / (far-near));
|
|
matrixOut[3][2] = float(near / (near-far));
|
|
|
|
matrixOut[3][3] = 1.0f;
|
|
|
|
mcpy(destMatrix, matrixOut, 64);
|
|
}
|
|
|
|
void Matrix4x4Frustum(float *destMatrix, double left, double right, double bottom, double top, double near, double far)
|
|
{
|
|
float matrixOut[4][4];
|
|
zero(matrixOut, 64);
|
|
|
|
matrixOut[0][0] = float((2.0*near) / (right-left));
|
|
matrixOut[2][0] = float((left+right) / (left-right));
|
|
|
|
matrixOut[1][1] = float((2.0*near) / (top-bottom));
|
|
matrixOut[2][1] = float((top+bottom) / (bottom-top));
|
|
|
|
matrixOut[2][2] = float(far / (far-near));
|
|
matrixOut[3][2] = float((near*far) / (near-far));
|
|
|
|
matrixOut[2][3] = 1.0f;
|
|
|
|
mcpy(destMatrix, matrixOut, 64);
|
|
}
|
|
|
|
void Matrix4x4Perspective(float *destMatrix, float angle, float aspect, float near, float far)
|
|
{
|
|
float xmin, xmax, ymin, ymax;
|
|
|
|
ymax = near * tanf(RAD(angle)*0.5f);
|
|
ymin = -ymax;
|
|
|
|
xmin = ymin * aspect;
|
|
xmax = ymax * aspect;
|
|
|
|
Matrix4x4Frustum(destMatrix, xmin, xmax, ymin, ymax, near, far);
|
|
}
|