1155 lines
30 KiB
C++
1155 lines
30 KiB
C++
|
////////////////////////////////////////////////////////////////////////////////////////
|
||
|
|
// 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;
|
||
|
|
}
|
||
|
|
|