raven/jediacademy
0
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity
jediacademy/code/Ravl/CVec.cpp

1155 lines
30 KiB
C++
Raw Permalink Normal View History

Jedi Academy Release
2013-04-04 22:35:38 +00:00
////////////////////////////////////////////////////////////////////////////////////////
// RAVEN STANDARD TEMPLATE LIBRARY
// (c) 2002 Activision
//
//
// Vector Library
////////////////////////////////////////////////////////////////////////////////////////
#include <assert.h>
#include <math.h>
#include <stdio.h>
#include <float.h>
#include "CVec.h"
//using namespace ravl;
////////////////////////////////////////////////////////////////////////////////////////
// Static Class Member Initialization
////////////////////////////////////////////////////////////////////////////////////////
const CVec4 CVec4::mX(1.f, 0.f, 0.f, 0.f);
const CVec4 CVec4::mY(0.f, 1.f, 0.f, 0.f);
const CVec4 CVec4::mZ(0.f, 0.f, 1.f, 0.f);
const CVec4 CVec4::mW(0.f, 0.f, 0.f, 1.f);
const CVec4 CVec4::mZero(0.f, 0.f, 0.f, 0.f);
////////////////////////////////////////////////////////////////////////////////////////
// Length
////////////////////////////////////////////////////////////////////////////////////////
float CVec4::Len() const
{
return sqrtf(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]+v[3]*v[3]);
}
////////////////////////////////////////////////////////////////////////////////////////
// Distance To Other Point
////////////////////////////////////////////////////////////////////////////////////////
float CVec4::Dist(const CVec4& t) const
{
return sqrtf(
(t.v[0]-v[0])*(t.v[0]-v[0]) +
(t.v[1]-v[1])*(t.v[1]-v[1]) +
(t.v[2]-v[2])*(t.v[2]-v[2]) +
(t.v[3]-v[3])*(t.v[3]-v[3]));
}
////////////////////////////////////////////////////////////////////////////////////////
// Normalize
////////////////////////////////////////////////////////////////////////////////////////
float CVec4::Norm()
{
float L = Len();
(*this)/=L;
return L;
}
////////////////////////////////////////////////////////////////////////////////////////
// Safe Normalize
// Do Not Normalize If Length Is Too Small
////////////////////////////////////////////////////////////////////////////////////////
float CVec4::SafeNorm()
{
float d=this->Len();
if (d>1E-10)
{
(*this)/=d;
return d;
}
(*this)=0.0f;
return 0;
}
////////////////////////////////////////////////////////////////////////////////////////
// Angular Normalize
// All floats Exist(-180, +180)
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::AngleNorm()
{
v[0]= fmodf(v[0],360.0f);
if (v[0]<-180.f) v[0]+=360.0f;
if (v[0]>180.f) v[0]-=360.0f;
v[1]= fmodf(v[1],360.0f);
if (v[1]<-180.f) v[1]+=360.0f;
if (v[1]>180.f) v[1]-=360.0f;
v[2]= fmodf(v[2],360.0f);
if (v[2]<-180.f) v[2]+=360.0f;
if (v[2]>180.f) v[2]-=360.0f;
}
////////////////////////////////////////////////////////////////////////////////////////
// Perpendicular Vector
//
// This implimentation is a bit slow, needs some optimization work...
//
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::Perp()
{
float rlen,tlen;
CVec4 r,t;
r = (*this);
r.Cross(mX);
rlen=r.Len();
t = (*this);
t.Cross(mY);
tlen=t.Len();
if (tlen>rlen)
{
r=t;
rlen=tlen;
}
t = (*this);
t.Cross(mZ);
tlen=t.Len();
if (tlen>rlen)
{
r=t;
rlen=tlen;
}
(*this) = r;
}
////////////////////////////////////////////////////////////////////////////////////////
// Find Largest Element (Ignores 4th component for now)
////////////////////////////////////////////////////////////////////////////////////////
int CVec4::MaxElementIndex() const
{
if (fabs(v[0])>fabs(v[1]) && fabs(v[0])>fabs(v[2]))
{
return 0;
}
if (fabs(v[1])>fabs(v[2]))
{
return 1;
}
return 2;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert To {Pitch, Yaw} (DEGREES)
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::VecToAng()
{
float yaw;
float pitch;
if (!v[1] && !v[0])
{
yaw = 0;
pitch = (v[2]>0) ? (90.0f):(270.0f);
}
else
{
// Calculate Yaw
//---------------
if (v[0])
{
yaw = (RAVL_VEC_RADTODEG(atan2f(v[1], v[0])));
if (yaw<0)
{
yaw += 360;
}
}
else
{
yaw = (v[1]>0) ? (90.0f):(270.0f);
}
// Calculate Pitch
//-----------------
float forward = (sqrtf(v[0]*v[0] + v[1]*v[1]));
pitch = (RAVL_VEC_RADTODEG(atan2f(v[2], forward)));
if (pitch<0)
{
pitch += 360;
}
}
// Copy Over Current Vector
//--------------------------
v[0] = -pitch;
v[1] = yaw;
v[2] = 0;
v[3] = 0;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert From {Picth, Yaw} (DEGREES)
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::AngToVec()
{
float angle;
float sp, sy, cp, cy;
angle = yaw() * (RAVL_VEC_DEGTORADCONST);
sy = sinf(angle);
cy = cosf(angle);
angle = pitch() * (RAVL_VEC_DEGTORADCONST);
sp = sinf(angle);
cp = cosf(angle);
v[0] = cp * cy;
v[1] = cp * sy;
v[2] = -sp;
v[3] = 0;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert From {Picth, Yaw, Roll} (DEGREES)
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::AngToVec(CVec4& Right, CVec4& Up)
{
float angle;
float sr, sp, sy, cr, cp, cy;
angle = yaw() * (RAVL_VEC_DEGTORADCONST);
sy = sinf(angle);
cy = cosf(angle);
angle = pitch() * (RAVL_VEC_DEGTORADCONST);
sp = sinf(angle);
cp = cosf(angle);
angle = roll() * (RAVL_VEC_DEGTORADCONST);
sr = sinf(angle);
cr = cosf(angle);
// Forward Vector Is Stored Here
v[0] = cp * cy;
v[1] = cp * sy;
v[2] = -sp;
v[3] = 0;
// Calculate Right
Right.v[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
Right.v[1] = (-1 * sr * sp * sy + -1 * cr * cy);
Right.v[2] = -1 * sr * cp;
Right.v[3] = 0;
// Calculate Up
Up.v[0] = (cr * sp * cy + -sr * -sy);
Up.v[1] = (cr * sp * sy + -sr * cy);
Up.v[2] = cr * cp;
Up.v[3] = 0;
}
///////////////////////////////////////////////////////////////////////////////////////
// Convert To {Pitch, Yaw} (RADIANS)
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::VecToAngRad()
{
float yaw;
float pitch;
if (!v[1] && !v[0])
{
yaw = 0;
pitch = (v[2]>0) ? (RAVL_VEC_PI*0.5f):(RAVL_VEC_PI*1.5f);
}
else
{
// Calculate Yaw
//---------------
if (v[0])
{
yaw = (atan2f(v[1], v[0]));
}
else
{
yaw = (v[1]>0) ? (RAVL_VEC_PI*0.5f):(RAVL_VEC_PI*1.5f);
}
// Calculate Pitch
//-----------------
float forward = (sqrtf(v[0]*v[0] + v[1]*v[1]));
pitch = (atan2f(v[2], forward));
}
// Copy Over Current Vector
//--------------------------
v[0] = -pitch;
v[1] = yaw;
v[2] = 0;
v[3] = 0;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert From {Picth, Yaw} (RADIANS)
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::AngToVecRad()
{
float sp, sy, cp, cy;
sy = sinf(yaw());
cy = cosf(yaw());
sp = sinf(pitch());
cp = cosf(pitch());
v[0] = cp * cy;
v[1] = cp * sy;
v[2] = -sp;
v[3] = 0;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert From {Picth, Yaw, Roll} (RADIANS)
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::AngToVecRad(CVec4& Right, CVec4& Up)
{
float sr, sp, sy, cr, cp, cy;
sy = sinf(yaw());
cy = cosf(yaw());
sp = sinf(pitch());
cp = cosf(pitch());
sr = sinf(roll());
cr = cosf(roll());
// Forward Vector Is Stored Here
v[0] = cp * cy;
v[1] = cp * sy;
v[2] = -sp;
v[3] = 0;
// Calculate Right
Right.v[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
Right.v[1] = (-1 * sr * sp * sy + -1 * cr * cy);
Right.v[2] = -1 * sr * cp;
Right.v[3] = 0;
// Calculate Up
Up.v[0] = (cr * sp * cy + -sr * -sy);
Up.v[1] = (cr * sp * sy + -sr * cy);
Up.v[2] = cr * cp;
Up.v[3] = 0;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert To Radians
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::ToRadians()
{
v[0] = RAVL_VEC_DEGTORAD(v[0]);
v[1] = RAVL_VEC_DEGTORAD(v[1]);
v[2] = RAVL_VEC_DEGTORAD(v[2]);
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert To Degrees
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::ToDegrees()
{
v[0] = RAVL_VEC_RADTODEG(v[0]);
v[1] = RAVL_VEC_RADTODEG(v[1]);
v[2] = RAVL_VEC_RADTODEG(v[2]);
}
////////////////////////////////////////////////////////////////////////////////////////
// Copy Values From String
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::FromStr(const char *s)
{
// assert(s && s[0]);
sscanf(s, "(%f %f %f %f)", &v[0], &v[1], &v[2], &v[3]);
}
////////////////////////////////////////////////////////////////////////////////////////
// Write Values To A String
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::ToStr(char* s) const
{
// assert(s);
sprintf(s, "(%3.3f %3.3f %3.3f %3.3f)", v[0], v[1], v[2], v[3]);
}
#ifdef _DEBUG
////////////////////////////////////////////////////////////////////////////////////////
// Make Sure Entire Vector Has Valid Numbers
////////////////////////////////////////////////////////////////////////////////////////
bool CVec4::IsFinite()
{
#if defined(_MSC_VER)
return (_finite(v[0]) && _finite(v[1]) && _finite(v[2]) && _finite(v[3]));
#endif
#if defined(__MWERKS__)
//FIXME: _finite won't compile on the PS2, so commented it out for now, false seemed like the best option
return true;
#endif
}
////////////////////////////////////////////////////////////////////////////////////////
// Make Sure Vector Has Been Initialized
////////////////////////////////////////////////////////////////////////////////////////
bool CVec4::IsInitialized()
{
return (v[0]!=RAVL_VEC_UDF && v[1]!=RAVL_VEC_UDF && v[2]!=RAVL_VEC_UDF && v[3]!=RAVL_VEC_UDF);
}
#endif
////////////////////////////////////////////////////////////////////////////////////////
// Point In Circumscribed Circle (True/False)
//
// Returns true if the given point is within the circumscribed
// circle of the given ABC Triangle:
// _____
// / B \
// / / \ \
// | / \ |
// |A---------C|
// \ Pt /
// \_______/
//
////////////////////////////////////////////////////////////////////////////////////////
bool CVec4::PtInCircle(const CVec4 &A, const CVec4 &B, const CVec4 &C) const
{
float vol;
float ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz;
float bxdx, bydy, bzdz, cxdx, cydy, czdz;
float tolerance=0.00000005f;
ax = A.v[0];
ay = A.v[1];
az = ax*ax + ay*ay;
bx = B.v[0];
by = B.v[1];
bz = bx*bx + by*by;
cx = C.v[0];
cy = C.v[1];
cz = cx*cx + cy*cy;
dx = v[0];
dy = v[1];
dz = dx*dx + dy*dy;
bxdx=bx-dx;
bydy=by-dy;
bzdz=bz-dz;
cxdx=cx-dx;
cydy=cy-dy;
czdz=cz-dz;
vol = (az-dz)*(bxdx*cydy-bydy*cxdx) + (ay-dy)*(bzdz*cxdx-bxdx*czdz) + (ax-dx)*(bydy*czdz-bzdz*cydy);
if ( vol > tolerance) return true;
else if ( vol < -1*tolerance) return false;
else return false;
}
////////////////////////////////////////////////////////////////////////////////////////
// Point In Standard Circle (True/False)
//
// Returns true if the given point is within the Circle
// _____
// / \
// / \
// | Circle |
// | |
// \ Pt /
// \_______/
//
////////////////////////////////////////////////////////////////////////////////////////
bool CVec4::PtInCircle(const CVec4 &Circle, float Radius) const
{
return (Dist2(Circle)<(Radius*Radius));
}
////////////////////////////////////////////////////////////////////////////////////////
// Line Intersects Circle (True/False)
//
// r - Radius Of The Circle
// A - Start Of Line Segment
// B - End Of Line Segment
//
// P - Projected Position Of Origin Onto Line AB
//
//
// B
// /
// /
// P
// / \ \
// / this-r->|
// / /
// A
//
////////////////////////////////////////////////////////////////////////////////////////
bool CVec4::LineInCircle(const CVec4 &A, const CVec4 &B, float r, CVec4 &P)
{
P = (*this);
float Scale = P.ProjectToLine(A, B);
// If The Projected Position Is Not On The Line Segment,
// Check If It Is Within Radius Of Endpoints A and B.
//-------------------------------------------------------
if (Scale<0 || Scale>1)
{
return (PtInCircle(A, r) || PtInCircle(B, r));
}
// Otherwise, Check To See If P Is Within The Radius Of This Circle
//------------------------------------------------------------------
return (PtInCircle(P, r));
}
////////////////////////////////////////////////////////////////////////////////////////
// Same As Test Above, Just Don't Bother Returning P
////////////////////////////////////////////////////////////////////////////////////////
bool CVec4::LineInCircle(const CVec4 &A, const CVec4 &B, float r)
{
CVec4 P(*this);
float Scale = P.ProjectToLine(A, B);
// If The Projected Position Is Not On The Line Segment,
// Check If It Is Within Radius Of Endpoints A and B.
//-------------------------------------------------------
if (Scale<0 || Scale>1)
{
return (PtInCircle(A, r) || PtInCircle(B, r));
}
// Otherwise, Check To See If P Is Within The Radius Of This Circle
//------------------------------------------------------------------
return (PtInCircle(P, r));
}
////////////////////////////////////////////////////////////////////////////////////////
// Rotate
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::RotatePoint(const CVec4 &, const CVec4 &)
{
// TO DO: Use Matrix Code To Rotate
}
////////////////////////////////////////////////////////////////////////////////////////
// Reposition
////////////////////////////////////////////////////////////////////////////////////////
void CVec4::Reposition(const CVec4 &Translation, float RotationDegrees)
{
// Apply Any Rotation First
//--------------------------
if (RotationDegrees)
{
CVec4 Old(*this);
float Rotation = RAVL_VEC_DEGTORAD(RotationDegrees);
v[0] = Old.v[0]*cosf(Rotation) - Old.v[1]*sinf(Rotation);
v[1] = Old.v[0]*sinf(Rotation) + Old.v[1]*cosf(Rotation);
}
// Now Apply Translation
//-----------------------
(*this) += Translation;
}
////////////////////////////////////////////////////////////////////////////////////////
// Static Class Member Initialization
////////////////////////////////////////////////////////////////////////////////////////
const CVec3 CVec3::mX(1.f, 0.f, 0.f);
const CVec3 CVec3::mY(0.f, 1.f, 0.f);
const CVec3 CVec3::mZ(0.f, 0.f, 1.f);
const CVec3 CVec3::mZero(0.f, 0.f, 0.f);
////////////////////////////////////////////////////////////////////////////////////////
// Length
////////////////////////////////////////////////////////////////////////////////////////
float CVec3::Len() const
{
return sqrtf(v[0]*v[0]+v[1]*v[1]+v[2]*v[2]);
}
////////////////////////////////////////////////////////////////////////////////////////
// Distance To Other Point
////////////////////////////////////////////////////////////////////////////////////////
float CVec3::Dist(const CVec3& t) const
{
return sqrtf(
(t.v[0]-v[0])*(t.v[0]-v[0]) +
(t.v[1]-v[1])*(t.v[1]-v[1]) +
(t.v[2]-v[2])*(t.v[2]-v[2]));
}
////////////////////////////////////////////////////////////////////////////////////////
// Normalize
////////////////////////////////////////////////////////////////////////////////////////
float CVec3::Norm()
{
float L = Len();
(*this)/=L;
return L;
}
////////////////////////////////////////////////////////////////////////////////////////
// Safe Normalize
// Do Not Normalize If Length Is Too Small
////////////////////////////////////////////////////////////////////////////////////////
float CVec3::SafeNorm()
{
float d=this->Len();
if (d>1E-10)
{
(*this)/=d;
return d;
}
(*this)=0.0f;
return 0;
}
////////////////////////////////////////////////////////////////////////////////////////
// Angular Normalize
// All floats Exist(-180, +180)
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::AngleNorm()
{
v[0]= fmodf(v[0],360.0f);
if (v[0]<-180.f) v[0]+=360.0f;
if (v[0]>180.f) v[0]-=360.0f;
v[1]= fmodf(v[1],360.0f);
if (v[1]<-180.f) v[1]+=360.0f;
if (v[1]>180.f) v[1]-=360.0f;
v[2]= fmodf(v[2],360.0f);
if (v[2]<-180.f) v[2]+=360.0f;
if (v[2]>180.f) v[2]-=360.0f;
}
////////////////////////////////////////////////////////////////////////////////////////
// Angular Normalize
// All floats Exist(-180, +180)
////////////////////////////////////////////////////////////////////////////////////////
float CVec3::Truncate(float maxlen)
{
float len=this->Len();
if (len>maxlen && len>1E-10)
{
len = maxlen / len;
v[0] *= len;
v[1] *= len;
v[2] *= len;
return maxlen;
}
return len;
}
////////////////////////////////////////////////////////////////////////////////////////
// Perpendicular Vector
//
// This implimentation is a bit slow, needs some optimization work...
//
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::Perp()
{
float rlen,tlen;
CVec3 r,t;
r = (*this);
r.Cross(mX);
rlen=r.Len();
t = (*this);
t.Cross(mY);
tlen=t.Len();
if (tlen>rlen)
{
r=t;
rlen=tlen;
}
t = (*this);
t.Cross(mZ);
tlen=t.Len();
if (tlen>rlen)
{
r=t;
rlen=tlen;
}
(*this) = r;
}
////////////////////////////////////////////////////////////////////////////////////////
// Find Largest Element (Ignores 4th component for now)
////////////////////////////////////////////////////////////////////////////////////////
int CVec3::MaxElementIndex() const
{
if (fabs(v[0])>fabs(v[1]) && fabs(v[0])>fabs(v[2]))
{
return 0;
}
if (fabs(v[1])>fabs(v[2]))
{
return 1;
}
return 2;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert To {Pitch, Yaw} (DEGREES)
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::VecToAng()
{
float yaw;
float pitch;
if (!v[1] && !v[0])
{
yaw = 0;
pitch = (v[2]>0) ? (90.0f):(270.0f);
}
else
{
// Calculate Yaw
//---------------
if (v[0])
{
yaw = (RAVL_VEC_RADTODEG(atan2f(v[1], v[0])));
if (yaw<0)
{
yaw += 360;
}
}
else
{
yaw = (v[1]>0) ? (90.0f):(270.0f);
}
// Calculate Pitch
//-----------------
float forward = (sqrtf(v[0]*v[0] + v[1]*v[1]));
pitch = (RAVL_VEC_RADTODEG(atan2f(v[2], forward)));
if (pitch<0)
{
pitch += 360;
}
}
// Copy Over Current Vector
//--------------------------
v[0] = -pitch;
v[1] = yaw;
v[2] = 0;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert From {Picth, Yaw} (DEGREES)
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::AngToVec()
{
float angle;
float sp, sy, cp, cy;
angle = yaw() * (RAVL_VEC_DEGTORADCONST);
sy = sinf(angle);
cy = cosf(angle);
angle = pitch() * (RAVL_VEC_DEGTORADCONST);
sp = sinf(angle);
cp = cosf(angle);
v[0] = cp * cy;
v[1] = cp * sy;
v[2] = -sp;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert From {Picth, Yaw, Roll} (DEGREES)
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::AngToVec(CVec3& Right, CVec3& Up)
{
float angle;
float sr, sp, sy, cr, cp, cy;
angle = yaw() * (RAVL_VEC_DEGTORADCONST);
sy = sinf(angle);
cy = cosf(angle);
angle = pitch() * (RAVL_VEC_DEGTORADCONST);
sp = sinf(angle);
cp = cosf(angle);
angle = roll() * (RAVL_VEC_DEGTORADCONST);
sr = sinf(angle);
cr = cosf(angle);
// Forward Vector Is Stored Here
v[0] = cp * cy;
v[1] = cp * sy;
v[2] = -sp;
// Calculate Right
Right.v[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
Right.v[1] = (-1 * sr * sp * sy + -1 * cr * cy);
Right.v[2] = -1 * sr * cp;
// Calculate Up
Up.v[0] = (cr * sp * cy + -sr * -sy);
Up.v[1] = (cr * sp * sy + -sr * cy);
Up.v[2] = cr * cp;
}
///////////////////////////////////////////////////////////////////////////////////////
// Convert To {Pitch, Yaw} (RADIANS)
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::VecToAngRad()
{
float yaw;
float pitch;
if (!v[1] && !v[0])
{
yaw = 0;
pitch = (v[2]>0) ? (RAVL_VEC_PI*0.5f):(RAVL_VEC_PI*1.5f);
}
else
{
// Calculate Yaw
//---------------
if (v[0])
{
yaw = (atan2f(v[1], v[0]));
}
else
{
yaw = (v[1]>0) ? (RAVL_VEC_PI*0.5f):(RAVL_VEC_PI*1.5f);
}
// Calculate Pitch
//-----------------
float forward = (sqrtf(v[0]*v[0] + v[1]*v[1]));
pitch = (atan2f(v[2], forward));
}
// Copy Over Current Vector
//--------------------------
v[0] = -pitch;
v[1] = yaw;
v[2] = 0;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert From {Picth, Yaw} (RADIANS)
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::AngToVecRad()
{
float sp, sy, cp, cy;
sy = sinf(yaw());
cy = cosf(yaw());
sp = sinf(pitch());
cp = cosf(pitch());
v[0] = cp * cy;
v[1] = cp * sy;
v[2] = -sp;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert From {Picth, Yaw, Roll} (RADIANS)
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::AngToVecRad(CVec3& Right, CVec3& Up)
{
float sr, sp, sy, cr, cp, cy;
sy = sinf(yaw());
cy = cosf(yaw());
sp = sinf(pitch());
cp = cosf(pitch());
sr = sinf(roll());
cr = cosf(roll());
// Forward Vector Is Stored Here
v[0] = cp * cy;
v[1] = cp * sy;
v[2] = -sp;
// Calculate Right
Right.v[0] = (-1 * sr * sp * cy + -1 * cr * -sy);
Right.v[1] = (-1 * sr * sp * sy + -1 * cr * cy);
Right.v[2] = -1 * sr * cp;
// Calculate Up
Up.v[0] = (cr * sp * cy + -sr * -sy);
Up.v[1] = (cr * sp * sy + -sr * cy);
Up.v[2] = cr * cp;
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert To Radians
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::ToRadians()
{
v[0] = RAVL_VEC_DEGTORAD(v[0]);
v[1] = RAVL_VEC_DEGTORAD(v[1]);
v[2] = RAVL_VEC_DEGTORAD(v[2]);
}
////////////////////////////////////////////////////////////////////////////////////////
// Convert To Degrees
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::ToDegrees()
{
v[0] = RAVL_VEC_RADTODEG(v[0]);
v[1] = RAVL_VEC_RADTODEG(v[1]);
v[2] = RAVL_VEC_RADTODEG(v[2]);
}
////////////////////////////////////////////////////////////////////////////////////////
// Copy Values From String
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::FromStr(const char *s)
{
assert(s && s[0]);
sscanf(s, "(%f %f %f)", &v[0], &v[1], &v[2]);
}
////////////////////////////////////////////////////////////////////////////////////////
// Write Values To A String
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::ToStr(char* s) const
{
assert(s);
sprintf(s, "(%3.3f %3.3f %3.3f)", v[0], v[1], v[2]);
}
#ifdef _DEBUG
////////////////////////////////////////////////////////////////////////////////////////
// Make Sure Entire Vector Has Valid Numbers
////////////////////////////////////////////////////////////////////////////////////////
bool CVec3::IsFinite()
{
#if defined(_MSC_VER)
return (_finite(v[0]) && _finite(v[1]) && _finite(v[2]));
#endif
#if defined(__MWERKS__)
//FIXME: _finite won't compile on the PS2, so commented it out for now, false seemed like the best option
return true;
#endif
}
////////////////////////////////////////////////////////////////////////////////////////
// Make Sure Vector Has Been Initialized
////////////////////////////////////////////////////////////////////////////////////////
bool CVec3::IsInitialized()
{
return (v[0]!=RAVL_VEC_UDF && v[1]!=RAVL_VEC_UDF && v[2]!=RAVL_VEC_UDF);
}
#endif
////////////////////////////////////////////////////////////////////////////////////////
// Point In Circumscribed Circle (True/False)
//
// Returns true if the given point is within the circumscribed
// circle of the given ABC Triangle:
// _____
// / B \
// / / \ \
// | / \ |
// |A---------C|
// \ Pt /
// \_______/
//
////////////////////////////////////////////////////////////////////////////////////////
bool CVec3::PtInCircle(const CVec3 &A, const CVec3 &B, const CVec3 &C) const
{
float vol;
float ax, ay, az, bx, by, bz, cx, cy, cz, dx, dy, dz;
float bxdx, bydy, bzdz, cxdx, cydy, czdz;
float tolerance=0.00000005f;
ax = A.v[0];
ay = A.v[1];
az = ax*ax + ay*ay;
bx = B.v[0];
by = B.v[1];
bz = bx*bx + by*by;
cx = C.v[0];
cy = C.v[1];
cz = cx*cx + cy*cy;
dx = v[0];
dy = v[1];
dz = dx*dx + dy*dy;
bxdx=bx-dx;
bydy=by-dy;
bzdz=bz-dz;
cxdx=cx-dx;
cydy=cy-dy;
czdz=cz-dz;
vol = (az-dz)*(bxdx*cydy-bydy*cxdx) + (ay-dy)*(bzdz*cxdx-bxdx*czdz) + (ax-dx)*(bydy*czdz-bzdz*cydy);
if ( vol > tolerance) return true;
else if ( vol < -1*tolerance) return false;
else return false;
}
////////////////////////////////////////////////////////////////////////////////////////
// Point In Standard Circle (True/False)
//
// Returns true if the given point is within the Circle
// _____
// / \
// / \
// | Circle |
// | |
// \ Pt /
// \_______/
//
////////////////////////////////////////////////////////////////////////////////////////
bool CVec3::PtInCircle(const CVec3 &Circle, float Radius) const
{
return (Dist2(Circle)<(Radius*Radius));
}
////////////////////////////////////////////////////////////////////////////////////////
// Line Intersects Circle (True/False)
//
// r - Radius Of The Circle
// A - Start Of Line Segment
// B - End Of Line Segment
//
// P - Projected Position Of Origin Onto Line AB
//
//
// B
// /
// /
// P
// / \ \
// / this-r->|
// / /
// A
//
////////////////////////////////////////////////////////////////////////////////////////
bool CVec3::LineInCircle(const CVec3 &A, const CVec3 &B, float r, CVec3 &P)
{
P = (*this);
float Scale = P.ProjectToLine(A, B);
// If The Projected Position Is Not On The Line Segment,
// Check If It Is Within Radius Of Endpoints A and B.
//-------------------------------------------------------
if (Scale<0 || Scale>1)
{
return (PtInCircle(A, r) || PtInCircle(B, r));
}
// Otherwise, Check To See If P Is Within The Radius Of This Circle
//------------------------------------------------------------------
return (PtInCircle(P, r));
}
////////////////////////////////////////////////////////////////////////////////////////
// Same As Test Above, Just Don't Bother Returning P
////////////////////////////////////////////////////////////////////////////////////////
bool CVec3::LineInCircle(const CVec3 &A, const CVec3 &B, float r)
{
CVec3 P(*this);
float Scale = P.ProjectToLine(A, B);
// If The Projected Position Is Not On The Line Segment,
// Check If It Is Within Radius Of Endpoints A and B.
//-------------------------------------------------------
if (Scale<0 || Scale>1)
{
return (PtInCircle(A, r) || PtInCircle(B, r));
}
// Otherwise, Check To See If P Is Within The Radius Of This Circle
//------------------------------------------------------------------
return (PtInCircle(P, r));
}
////////////////////////////////////////////////////////////////////////////////////////
// Rotate
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::RotatePoint(const CVec3 &, const CVec3 &)
{
// TO DO: Use Matrix Code To Rotate
}
////////////////////////////////////////////////////////////////////////////////////////
// Reposition
////////////////////////////////////////////////////////////////////////////////////////
void CVec3::Reposition(const CVec3 &Translation, float RotationDegrees)
{
// Apply Any Rotation First
//--------------------------
if (RotationDegrees)
{
CVec3 Old(*this);
float Rotation = RAVL_VEC_DEGTORAD(RotationDegrees);
v[0] = Old.v[0]*cosf(Rotation) - Old.v[1]*sinf(Rotation);
v[1] = Old.v[0]*sinf(Rotation) + Old.v[1]*cosf(Rotation);
}
// Now Apply Translation
//-----------------------
(*this) += Translation;
}
Reference in a new issue Copy permalink