806 lines
25 KiB
C++
806 lines
25 KiB
C++
//////////////////////////////////////////////////////////////////////
|
|
//
|
|
// Crytek Common Source code
|
|
//
|
|
// File: Cry_Vector3.h
|
|
// Description: Common vector class
|
|
//
|
|
// History:
|
|
// -Feb 27,2003: Created by Ivo Herzeg
|
|
//
|
|
//////////////////////////////////////////////////////////////////////
|
|
|
|
#ifndef VECTOR_H
|
|
#define VECTOR_H
|
|
|
|
#if _MSC_VER > 1000
|
|
# pragma once
|
|
#endif
|
|
|
|
#include "platform.h"
|
|
#include <math.h>
|
|
#include "Cry_Matrix.h"
|
|
|
|
enum type_zero { zero };
|
|
enum type_min { MIN };
|
|
enum type_max { MAX };
|
|
|
|
extern const float gf_PI;
|
|
|
|
#define VEC_EPSILON ( 0.01f )
|
|
#define DEG2RAD( a ) ( (a) * (gf_PI/180.0f) )
|
|
#define RAD2DEG( a ) ( (a) * (180.0f/gf_PI) )
|
|
|
|
|
|
#if defined(LINUX)
|
|
template <class F> struct Vec3_tpl;
|
|
template<class F>
|
|
F GetLengthSquared( const Vec3_tpl<F> &v );
|
|
template<class F>
|
|
F GetLength( const Vec3_tpl<F>& v );
|
|
#endif
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// class Vec3_tpl
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <class F> struct Vec3_tpl
|
|
{
|
|
public:
|
|
|
|
F x,y,z;
|
|
|
|
Vec3_tpl() {};
|
|
Vec3_tpl(type_zero) { x=y=z=0; }
|
|
Vec3_tpl(type_min) { x=y=z=-100; }
|
|
Vec3_tpl(type_max) { x=y=z=100; }
|
|
|
|
Vec3_tpl( const F vx, const F vy, const F vz ) { x=vx; y=vy; z=vz; };
|
|
|
|
//bracket operator
|
|
void operator () ( const F vx,const F vy,const F vz ) { x=vx; y=vy; z=vz; };
|
|
|
|
//f32* fptr=vec;
|
|
operator F* () { return (F*)this; }
|
|
|
|
//f32 farray[3]={1,2,3};
|
|
//Vec3 v=Vec3(farray);
|
|
//
|
|
//f32* fptr;
|
|
//Vec3 newv=Vec3(fptr);
|
|
//
|
|
//Matrix44 tm=GetTranslationMat(Vec3(44,55,66));
|
|
//Vec3 tvec=Vec3(tm[3]); //really dangerous! Should be replaced by "GetTranslation(tm)"
|
|
template <class T>
|
|
explicit Vec3_tpl(const T *src) { x=src[0]; y=src[1]; z=src[2]; }
|
|
|
|
//CONSTRUCTOR: implement the copy/casting/assignement constructor:
|
|
template <class T>
|
|
ILINE Vec3_tpl( const Vec3_tpl<T>& v ) { x=(F)v.x; y=(F)v.y; z=(F)v.z; }
|
|
|
|
//overload = operator to copy f64=doube or f32=f32
|
|
//Vec3_tpl<f32>=Vec3_tpl<f32>
|
|
//Vec3_tpl<f64>=Vec3_tpl<f64>
|
|
template <class T>
|
|
ILINE Vec3_tpl& operator=(const Vec3_tpl<T> &v) { x=v.x; y=v.y; z=v.z; return *this; }
|
|
|
|
|
|
ILINE F &operator [] (int index) { assert(index>=0 && index<=2); return ((F*)this)[index]; }
|
|
ILINE F operator [] (int index) const { assert(index>=0 && index<=2); return ((F*)this)[index]; }
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////////////
|
|
//overloaded arithmetic operators
|
|
////////////////////////////////////////////////////////////////
|
|
|
|
//three methods for a "dot-product" operation
|
|
ILINE F Dot (const Vec3_tpl<F> &vec2) const { return x*vec2.x + y*vec2.y + z*vec2.z; }
|
|
//two methods for a "cross-product" operation
|
|
ILINE Vec3_tpl<F> Cross (const Vec3_tpl<F> &vec2) const { return Vec3_tpl<F>( y*vec2.z - z*vec2.y, z*vec2.x - x*vec2.z, x*vec2.y - y*vec2.x); }
|
|
|
|
|
|
ILINE Vec3_tpl<F> operator*(F k) const { return Vec3_tpl<F>(x*k,y*k,z*k); }
|
|
ILINE Vec3_tpl<F> operator/(F k) const { k=(F)1.0/k; return Vec3_tpl<F>(x*k,y*k,z*k); }
|
|
ILINE friend Vec3_tpl operator * (f32 f, const Vec3_tpl &vec) { return Vec3_tpl(f*vec.x, f*vec.y, f*vec.z); }
|
|
|
|
|
|
ILINE Vec3_tpl<F>& operator *= (F k) { x*=k;y*=k;z*=k; return *this; }
|
|
ILINE Vec3_tpl<F>& operator /= (F k) { k=(F)1.0/k; x*=k;y*=k;z*=k; return *this; }
|
|
|
|
|
|
|
|
//flip vector
|
|
ILINE Vec3_tpl<F> operator - ( void ) const { return Vec3_tpl<F>(-x,-y,-z); }
|
|
// ILINE Vec3_tpl& Negate() { x=-x; y=-y; z=-z; return *this; }
|
|
ILINE Vec3_tpl& Flip() { x=-x; y=-y; z=-z; return *this; }
|
|
ILINE Vec3_tpl& flip() { x=-x; y=-y; z=-z; return *this; }
|
|
|
|
|
|
|
|
ILINE friend bool operator ==(const Vec3_tpl<F> &v0, const Vec3_tpl<F> &v1) {
|
|
return ((v0.x==v1.x) && (v0.y==v1.y) && (v0.z==v1.z));
|
|
}
|
|
ILINE friend bool operator !=(const Vec3_tpl<F> &v0, const Vec3_tpl<F> &v1) {
|
|
return !(v0==v1);
|
|
}
|
|
ILINE friend bool IsEquivalent(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1, f32 epsilon ) {
|
|
return ((fabs_tpl(v0.x-v1.x) <= epsilon) && (fabs_tpl(v0.y-v1.y) <= epsilon)&& (fabs_tpl(v0.z-v1.z) <= epsilon));
|
|
}
|
|
ILINE friend bool IsEquivalent(const Vec3_tpl<F>& v0, const Vec3_tpl<F>& v1 ) {
|
|
return( IsEquivalent( v0, v1, VEC_EPSILON ) );
|
|
}
|
|
|
|
ILINE bool IsZero() const { return x == 0 && y == 0 && z == 0; }
|
|
|
|
////////////////////////////////////////////////////////////////
|
|
//common methods
|
|
////////////////////////////////////////////////////////////////
|
|
ILINE Vec3_tpl& zero() { x=y=z=0; return *this; }
|
|
ILINE Vec3_tpl<F>& Set(const F xval,const F yval, const F zval) { x=xval; y=yval; z=zval; return *this; }
|
|
|
|
//! calcultae the length of the vector
|
|
ILINE F Length() const { return sqrt_tpl(x*x+y*y+z*z); }
|
|
ILINE F GetLength() const { return sqrt_tpl(x*x+y*y+z*z); }
|
|
ILINE F len() const { return sqrt_tpl(x*x+y*y+z*z); }
|
|
|
|
//! calcultae the squared length of the vector
|
|
ILINE F GetLengthSquared() const { return x*x+y*y+z*z; }
|
|
ILINE F len2() const { return x*x +y*y + z*z; }
|
|
|
|
|
|
|
|
//! normalize the vector and return the inverted length if successfull
|
|
ILINE f32 Normalize() {
|
|
f32 fLen = (f32)GetLength();
|
|
//assert(fLen>0.0f);
|
|
if (fLen<0.00001f) return(0); //no good idea! not everybody will check this
|
|
f32 fInvLen=1.0f/fLen;
|
|
x*=fInvLen; y*=fInvLen; z*=fInvLen;
|
|
return fInvLen;
|
|
}
|
|
|
|
//! return a normalized vector
|
|
ILINE friend Vec3_tpl<F> GetNormalized( const Vec3_tpl<F> &v ) {
|
|
F vlength = ::GetLength(v);
|
|
assert(vlength>0.0f);
|
|
F ivlength=1.0f/vlength;
|
|
return (v*ivlength);
|
|
}
|
|
|
|
|
|
ILINE Vec3_tpl& normalize() {
|
|
F rlen=sqrt_tpl(x*x+y*y+z*z);
|
|
//assert(rlen>0.00001f);
|
|
if (rlen>0) { rlen=(F)1.0/rlen; x*=rlen;y*=rlen;z*=rlen; }
|
|
else Set(0,0,1); return *this;
|
|
}
|
|
ILINE Vec3_tpl normalized() const {
|
|
F rlen=sqrt_tpl(x*x+y*y+z*z);
|
|
//assert(rlen>0.00001f);
|
|
if (rlen>0) { rlen=(F)1.0/rlen; return Vec3_tpl(x*rlen,y*rlen,z*rlen); }
|
|
else return Vec3_tpl(0,0,1);
|
|
}
|
|
|
|
ILINE Vec3_tpl GetNormalized() const { return normalized(); } // vlad: all get functions should begin from Get...
|
|
|
|
ILINE f32 NormalizeFast() {
|
|
f32 fLen = x*x + y*y + z*z;
|
|
//assert(fLen>0.00001f);
|
|
unsigned int *n1 = (unsigned int *)&fLen;
|
|
unsigned int n = 0x5f3759df - (*n1 >> 1);
|
|
f32 *n2 = (f32 *)&n;
|
|
fLen = (1.5f - (fLen * 0.5f) * *n2 * *n2) * *n2;
|
|
x*=fLen; y*=fLen; z*=fLen;
|
|
return fLen;
|
|
}
|
|
|
|
|
|
|
|
ILINE friend F GetSquaredDistance(const Vec3_tpl<F> &vec1, const Vec3_tpl<F> &vec2) {
|
|
return (vec2.x-vec1.x)*(vec2.x-vec1.x)+(vec2.y-vec1.y)*(vec2.y-vec1.y)+(vec2.z-vec1.z)*(vec2.z-vec1.z);
|
|
}
|
|
ILINE friend F GetDistance(const Vec3_tpl<F> &vec1, const Vec3_tpl<F> &vec2) {
|
|
return sqrt_tpl((vec2.x-vec1.x)*(vec2.x-vec1.x)+(vec2.y-vec1.y)*(vec2.y-vec1.y)+(vec2.z-vec1.z)*(vec2.z-vec1.z));
|
|
}
|
|
|
|
ILINE F GetDistance(const Vec3_tpl<F> vec1) const {
|
|
return sqrt_tpl((x-vec1.x)*(x-vec1.x)+(y-vec1.y)*(y-vec1.y)+(z-vec1.z)*(z-vec1.z));
|
|
}
|
|
|
|
//! force vector length by normalizing it
|
|
//! 08/26/2002 optimized a little by M.M.
|
|
ILINE void SetLen(const f32 fLen) {
|
|
f32 fLenMe = GetLengthSquared();
|
|
if(fLenMe<0.00001f*0.00001f)return;
|
|
fLenMe=fLen/(f32)sqrt((f64)fLenMe);
|
|
x*=fLenMe; y*=fLenMe; z*=fLenMe;
|
|
}
|
|
|
|
|
|
// permutate coordinates so that z goes to new_z slot
|
|
ILINE Vec3_tpl permutated(int new_z) const { return Vec3_tpl(*(&x+inc_mod3[new_z]), *(&x+dec_mod3[new_z]), *(&x+new_z)); }
|
|
|
|
// returns volume of a box with this vector as diagonal
|
|
ILINE F volume() const { return x*y*z; }
|
|
|
|
// returns a vector orthogonal to this one
|
|
ILINE Vec3_tpl orthogonal() const {
|
|
int i = isneg(square((F)0.9)*GetLengthSquared()-x*x);
|
|
Vec3_tpl<F> res;
|
|
res[i]=0; res[incm3(i)]=(*this)[decm3(i)]; res[decm3(i)]=-(*this)[incm3(i)];
|
|
return res;
|
|
}
|
|
|
|
// returns a vector orthogonal to this one
|
|
ILINE void SetOrthogonal( const Vec3_tpl<F>& v ) {
|
|
int i = isneg(square((F)0.9)*v.GetLengthSquared()-v.x*v.x);
|
|
(*this)[i]=0; (*this)[incm3(i)]= v[decm3(i)]; (*this)[decm3(i)]=-v[incm3(i)];
|
|
}
|
|
|
|
// returns a vector that consists of absolute values of this one's coordinates
|
|
ILINE Vec3_tpl abs() const { return Vec3_tpl(fabs_tpl(x),fabs_tpl(y),fabs_tpl(z)); }
|
|
|
|
|
|
//! check for min bounds
|
|
ILINE void CheckMin(const Vec3_tpl &other) {
|
|
if (other.x<x) x=other.x;
|
|
if (other.y<y) y=other.y;
|
|
if (other.z<z) z=other.z;
|
|
}
|
|
|
|
//! check for max bounds
|
|
ILINE void CheckMax(const Vec3_tpl &other) {
|
|
if (other.x>x) x=other.x;
|
|
if (other.y>y) y=other.y;
|
|
if (other.z>z) z=other.z;
|
|
}
|
|
|
|
|
|
|
|
//this is a special case for CAngleAxis
|
|
ILINE Vec3_tpl rotated(const Vec3_tpl &axis, F angle) const {
|
|
return rotated(axis,cos_tpl(angle),sin_tpl(angle));
|
|
}
|
|
ILINE Vec3_tpl rotated(const Vec3_tpl &axis, F cosa,F sina) const {
|
|
Vec3_tpl zax = axis*(*this*axis);
|
|
Vec3_tpl xax = *this-zax;
|
|
Vec3_tpl yax = axis^xax;
|
|
return xax*cosa + yax*sina + zax;
|
|
}
|
|
|
|
ILINE Vec3_tpl rotated(const Vec3_tpl ¢er,const Vec3_tpl &axis, F cosa,F sina) const {
|
|
return center+(*this-center).rotated(axis,cosa,sina);
|
|
}
|
|
ILINE Vec3_tpl rotated(const Vec3_tpl ¢er,const Vec3_tpl &axis, F angle) const {
|
|
return center+(*this-center).rotated(axis,angle);
|
|
}
|
|
|
|
//! set random normalized vector (=random position on unit sphere)
|
|
void SetRandomDirection( void )
|
|
{
|
|
F Length2;
|
|
do{
|
|
x=( (F)rand() )/RAND_MAX*2-1;
|
|
y=( (F)rand() )/RAND_MAX*2-1;
|
|
z=( (F)rand() )/RAND_MAX*2-1;
|
|
Length2=len2();
|
|
} while(Length2>1.0f || Length2<0.0001f);
|
|
|
|
F InvScale=1/sqrt_tpl(Length2); // dividion by 0 is prevented
|
|
|
|
x*=InvScale;y*=InvScale;z*=InvScale;
|
|
}
|
|
|
|
|
|
/*!
|
|
* Project a point/vector on a (virtual) plane
|
|
* Consider we have a plane going through the origin.
|
|
* Because d=0 we need just the normal. The vector n is assumed to be a unit-vector.
|
|
*
|
|
* Example:
|
|
* Vec3 result=Vec3::CreateProjection( incident, normal );
|
|
*/
|
|
ILINE static Vec3_tpl CreateProjection( const Vec3_tpl& i, const Vec3_tpl& n ) { return i - n*(n|i); }
|
|
|
|
/*!
|
|
* Calculate a reflection vector. Vec3 n is assumed to be a unit-vector.
|
|
*
|
|
* Example:
|
|
* Vec3 result=Vec3::CreateReflection( incident, normal );
|
|
*/
|
|
ILINE static Vec3_tpl CreateReflection( const Vec3_tpl& i, const Vec3_tpl &n ) { return (n*(i|n)*2)-i; }
|
|
|
|
};
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// Typedefs //
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
typedef Vec3_tpl<f32> Vec3; //we will use only this throughout the project
|
|
typedef Vec3_tpl<real> Vec3_f64;
|
|
|
|
|
|
typedef Vec3 Vec3d; //obsolete! please use just Vec3
|
|
typedef Vec3 vectorf;
|
|
typedef Vec3_f64 vectorr;
|
|
typedef Vec3_tpl<int> vectori;
|
|
|
|
|
|
inline Vec3_tpl<f32>::Vec3_tpl(type_min) { x=y=z=-3.3E38f; }
|
|
inline Vec3_tpl<f32>::Vec3_tpl(type_max) { x=y=z=3.3E38f; }
|
|
inline Vec3_tpl<f64>::Vec3_tpl(type_min) { x=y=z=-1.7E308; }
|
|
inline Vec3_tpl<f64>::Vec3_tpl(type_max) { x=y=z=1.7E308; }
|
|
|
|
template<class F>
|
|
ILINE F GetLengthSquared( const Vec3_tpl<F> &v ) { return v.x*v.x + v.y*v.y + v.z*v.z; }
|
|
template<class F>
|
|
ILINE F GetLength( const Vec3_tpl<F>& v ) { return sqrt_tpl(v.x*v.x + v.y*v.y + v.z*v.z); }
|
|
|
|
|
|
// dot product (2 versions)
|
|
template<class F1,class F2>
|
|
ILINE F1 operator * (const Vec3_tpl<F1> &v0, const Vec3_tpl<F2> &v1) {
|
|
return (F1)(v0.x*v1.x+v0.y*v1.y+v0.z*v1.z);
|
|
}
|
|
template<class F1,class F2>
|
|
ILINE F1 operator | (const Vec3_tpl<F1> &v0, const Vec3_tpl<F2> &v1) {
|
|
return v0.x*v1.x+v0.y*v1.y+v0.z*v1.z;
|
|
}
|
|
|
|
// cross product
|
|
template<class F1,class F2>
|
|
ILINE Vec3_tpl<F1> operator ^ (const Vec3_tpl<F1> &v0, const Vec3_tpl<F2> &v1) {
|
|
return Vec3_tpl<F1>(v0.y*v1.z-v0.z*v1.y, v0.z*v1.x-v0.x*v1.z, v0.x*v1.y-v0.y*v1.x);
|
|
}
|
|
template<class F1,class F2>
|
|
ILINE Vec3_tpl<F1> operator % (const Vec3_tpl<F1> &v0, const Vec3_tpl<F2> &v1) {
|
|
return Vec3_tpl<F1>(v0.y*v1.z-v0.z*v1.y, v0.z*v1.x-v0.x*v1.z, v0.x*v1.y-v0.y*v1.x);
|
|
}
|
|
|
|
|
|
//---------------------------------------------------------------------------
|
|
|
|
//vector addition
|
|
template<class F1,class F2>
|
|
ILINE Vec3_tpl<F1> operator + (const Vec3_tpl<F1> &v0, const Vec3_tpl<F2> &v1) {
|
|
return Vec3_tpl<F1>(v0.x+v1.x, v0.y+v1.y, v0.z+v1.z);
|
|
}
|
|
//vector subtraction
|
|
template<class F1,class F2>
|
|
ILINE Vec3_tpl<F1> operator - (const Vec3_tpl<F1> &v0, const Vec3_tpl<F2> &v1) {
|
|
return Vec3_tpl<F1>(v0.x-v1.x, v0.y-v1.y, v0.z-v1.z);
|
|
}
|
|
|
|
|
|
//---------------------------------------------------------------------------
|
|
|
|
|
|
//vector self-addition
|
|
template<class F1,class F2>
|
|
ILINE Vec3_tpl<F1>& operator += (Vec3_tpl<F1> &v0, const Vec3_tpl<F2> &v1) {
|
|
v0.x+=v1.x; v0.y+=v1.y; v0.z+=v1.z; return v0;
|
|
}
|
|
//vector self-subtraction
|
|
template<class F1,class F2>
|
|
ILINE Vec3_tpl<F1>& operator -= (Vec3_tpl<F1> &v0, const Vec3_tpl<F2> &v1) {
|
|
v0.x-=v1.x; v0.y-=v1.y; v0.z-=v1.z; return v0;
|
|
}
|
|
|
|
//! calculate the angle between two vectors in 3d (by M.M.)
|
|
//! /param invA direction of vector a (don't have to be normalized)
|
|
//! /param invB direction of vector b (don't have to be normalized)
|
|
//! /return angle in the range 0 to p radians.
|
|
ILINE f32 CalcAngleBetween( const Vec3 &invA, const Vec3 &invB ) {
|
|
f64 LengthQ=GetLength(invA)*GetLength(invB);
|
|
if(LengthQ<0.01)LengthQ=0.01;
|
|
return((f32)(acos((invA*invB)/LengthQ)));
|
|
}
|
|
|
|
// returns a vector orthogonal to this one
|
|
template<class F>
|
|
ILINE Vec3_tpl<F> GetOrthogonal( const Vec3_tpl<F>& v ) {
|
|
int i = isneg(square((F)0.9)*GetLengthSquared(v)-v.x*v.x);
|
|
Vec3_tpl<F> res;
|
|
res[i]=0; res[incm3(i)]= v[decm3(i)]; res[decm3(i)]=-v[incm3(i)];
|
|
return res;
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// struct Ang3_tpl
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
|
|
|
|
template <class F>
|
|
struct Ang3_tpl : public Vec3_tpl<F>
|
|
{
|
|
Ang3_tpl() {}
|
|
|
|
Ang3_tpl(const Vec3 &v) { this->x=v.x; this->y=v.y; this->z=v.z; }
|
|
Ang3_tpl &operator = (const Vec3 &v) { this->x=v.x; this->y=v.y; this->z=v.z; return *this; }
|
|
ILINE Ang3_tpl( const F vx, const F vy, const F vz ) { this->x=vx; this->y=vy; this->z=vz; }
|
|
//! normalize the vector ANGLE to -180, 180 range
|
|
ILINE void Snap180()
|
|
{
|
|
this->x = Snap_s180(this->x);
|
|
this->y = Snap_s180(this->y);
|
|
this->z = Snap_s180(this->z);
|
|
}
|
|
|
|
//! normalize the vector ANGLE to 0-360 range
|
|
ILINE void Snap360()
|
|
{
|
|
this->x = Snap_s360(this->x);
|
|
this->y = Snap_s360(this->y);
|
|
this->z = Snap_s360(this->z);
|
|
}
|
|
|
|
ILINE void Rad2Deg() { this->x=RAD2DEG(this->x); this->y=RAD2DEG(this->y); this->z=RAD2DEG(this->z); }
|
|
//! convert from degrees to radians
|
|
//ILINE void Deg2Rad() { this->x=DEG2RAD(this->x); this->y=DEG2RAD(this->y); this->z=DEG2RAD(this->z); }
|
|
|
|
template<int SI,int SJ>
|
|
ILINE void SetAnglesXYZ( const Matrix33_tpl<F,SI,SJ>& m )
|
|
{
|
|
//check if we have an orthonormal-base (assuming we are using a right-handed coordinate system)
|
|
assert( IsEquivalent(m.GetOrtX(),m.GetOrtY()%m.GetOrtZ(),0.1f) );
|
|
assert( IsEquivalent(m.GetOrtY(),m.GetOrtZ()%m.GetOrtX(),0.1f) );
|
|
assert( IsEquivalent(m.GetOrtZ(),m.GetOrtX()%m.GetOrtY(),0.1f) );
|
|
this->y = asin_tpl(MAX((F)-1.0,MIN((F)1.0,-m.data[SI*2+SJ*0])));
|
|
if (fabs_tpl(fabs_tpl(this->y)-(F)(pi*0.5))<(F)0.01)
|
|
{
|
|
this->x = 0;
|
|
this->z = atan2_tpl(-m.data[SI*0+SJ*1],m.data[SI*1+SJ*1]);
|
|
} else
|
|
{
|
|
this->x = atan2_tpl(m.data[SI*2+SJ*1], m.data[SI*2+SJ*2]);
|
|
this->z = atan2_tpl(m.data[SI*1+SJ*0], m.data[SI*0+SJ*0]);
|
|
}
|
|
}
|
|
template<int SI,int SJ>
|
|
ILINE static Ang3_tpl<F> GetAnglesXYZ( const Matrix33_tpl<F,SI,SJ>& m ) { Ang3_tpl<F> a; a.SetAnglesXYZ(m); return a; }
|
|
};
|
|
|
|
typedef Ang3_tpl<f32> Ang3;
|
|
|
|
//ILINE Ang3 Rad2Deg(const Ang3& a) { return Ang3(RAD2DEG(a.x),RAD2DEG(a.y),RAD2DEG(a.z)); }
|
|
ILINE Ang3 Deg2Rad(const Ang3& a) { return Ang3(DEG2RAD(a.x),DEG2RAD(a.y),DEG2RAD(a.z)); }
|
|
|
|
//! normalize the val to 0-360 range
|
|
ILINE f32 Snap_s360( f32 val ) {
|
|
if( val < 0.0f )
|
|
val =f32( 360.0f + cry_fmod(val,360.0f));
|
|
else
|
|
if(val >= 360.0f)
|
|
val =f32(cry_fmod(val,360.0f));
|
|
return val;
|
|
}
|
|
|
|
//! normalize the val to -180, 180 range
|
|
ILINE f32 Snap_s180( f32 val ) {
|
|
if( val > -180.0f && val < 180.0f)
|
|
return val;
|
|
val = Snap_s360( val );
|
|
if( val>180.0f )
|
|
return -(360.0f - val);
|
|
return val;
|
|
}
|
|
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// struct CAngleAxis
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
template <class F> struct AngleAxis_tpl {
|
|
|
|
//! storage for the Angle&Axis coordinates.
|
|
F angle; Vec3_tpl<F> axis;
|
|
|
|
// default quaternion constructor
|
|
AngleAxis_tpl( void ) { };
|
|
AngleAxis_tpl( const f32 a, const f32 ax, const f32 ay, const f32 az ) { angle=a; axis.x=ax; axis.y=ay; axis.z=az; }
|
|
AngleAxis_tpl( const f32 a, const Vec3_tpl<F> &n ) { angle=a; axis=n; }
|
|
void operator () ( const f32 a, const Vec3_tpl<F> &n ) { angle=a; axis=n; }
|
|
|
|
|
|
//CONSTRUCTOR: implement the copy/casting/assignement constructor:
|
|
AngleAxis_tpl( const AngleAxis_tpl<F>& aa ); //CAngleAxis aa=angleaxis
|
|
//explicit AngleAxis_tpl( const Matrix33_tpl<F>& m ); //CAngleAxis aa=m33
|
|
explicit AngleAxis_tpl( const Quaternion_tpl<F>& q );
|
|
|
|
const Vec3_tpl<F> operator * ( const Vec3_tpl<F>& v ) const;
|
|
|
|
};
|
|
|
|
typedef AngleAxis_tpl<f32> AngleAxis;
|
|
typedef AngleAxis_tpl<f64> AngleAxis_f64;
|
|
|
|
|
|
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
// CAngleAxis: Function & Operator declarations //
|
|
///////////////////////////////////////////////////////////////////////////////
|
|
|
|
/*!
|
|
*
|
|
* CAngleAxis copy constructor: CAngleAxis aa=mat33
|
|
* We take only the rotation of the 3x3 part
|
|
*
|
|
* Use with caution: this method suffers from some instability problems!
|
|
* If the rotation-radiant is near 0 or near 2pi we get an inaccurate result,
|
|
* because we have to normalize a very small vector!
|
|
* If "angle=~0" then any axis will do, since there is no rotation!
|
|
*
|
|
* Example:
|
|
* Matrix33 mat33 = rotationXYZ33(0.5f, 2.5f, 1.5f);
|
|
* CAngleAxis aa=mat33;
|
|
*/
|
|
/*template<class F>
|
|
ILINE AngleAxis_tpl<F>::AngleAxis_tpl( const Matrix33_tpl<F> &m ) {
|
|
|
|
angle = acos_tpl((m(0,0) + m(1,1) + m(2,2)-1.0f)*0.5f);
|
|
axis = GetNormalized( Vec3_tpl<F>(m(2,1)-m(1,2), m(0,2)-m(2,0), m(1,0)-m(0,1)) );
|
|
|
|
//if "angle=~0" any axis will do, since there is no rotation
|
|
if (angle<0.00001f) { axis(1,0,0); return; }
|
|
//if "angle~=pi" we need a special approach to get the axis
|
|
if (fabsf(angle-gf_PI)<0.00001f) {
|
|
//check if "m00" is the biggest element
|
|
if ( (m(0,0)>m(1,1)) && (m(0,0)>m(2,2)) ) {
|
|
axis.x = sqrtf( m(0,0) - m(1,1) - m(2,2) + 1.0f )/2;
|
|
axis.y = ( m(0,1) ) / (2*axis.x);
|
|
axis.z = ( m(0,2) ) / (2*axis.x);
|
|
return;
|
|
}
|
|
//check if "m11" is the biggest element
|
|
if ( (m(1,1)>m(0,0)) && (m(1,1)>m(2,2)) ) {
|
|
axis.x = sqrtf( m(1,1) - m(0,0) - m(2,2) + 1.0f )/2;
|
|
axis.y = ( m(0,1) ) / (2*axis.x);
|
|
axis.z = ( m(1,2) ) / (2*axis.x);
|
|
return;
|
|
}
|
|
//check if "m22" is the biggest element
|
|
if ( (m(2,2)>m(0,0)) && (m(2,2)>m(1,1)) ) {
|
|
axis.x = sqrtf( m(2,2) - m(0,0) - m(1,1) + 1.0f )/2;
|
|
axis.y = ( m(0,2) ) / (2*axis.x);
|
|
axis.z = ( m(1,2) ) / (2*axis.x);
|
|
return;
|
|
}
|
|
}
|
|
}*/
|
|
|
|
|
|
|
|
/*!
|
|
*
|
|
* CAngleAxisn copy constructor; CAngleAxis aa=q
|
|
*
|
|
* Use with caution: this method suffers from some instability problems!
|
|
* If the rotation-radiant is near 0 or near 2pi we get an inaccurate result,
|
|
* because we have to normalize a very small vector!
|
|
*
|
|
* Example:
|
|
* CQuaternion q;
|
|
* q.rotationXYZ(0.5f, 2.5f, 1.5f);
|
|
* CAngleAxis aa=mat33;
|
|
*/
|
|
template<class F>
|
|
ILINE AngleAxis_tpl<F>::AngleAxis_tpl( const Quaternion_tpl<F> &q ) {
|
|
// quaternion is assumed to be a unit-quaternion.
|
|
angle = acos_tpl(q.w)*2;
|
|
axis = q.v;
|
|
F sine = sin_tpl(angle*0.5f);
|
|
if ( sine ) { axis=q.v/sine; }
|
|
// if vector=0 then we assume this quaternion to be an identity-quaternion.
|
|
// an identity quaternion is a neutral element, so any normalized axis will do.
|
|
// we choose angle=0 and axis(1,0,0). Any arbitrary axis with zero-angle performs no rotation
|
|
if( (axis.x==0) && (axis.y==0) && (axis.z==0) ) { angle=0; axis.x=1; axis.y=0; axis.z=0; }
|
|
}
|
|
|
|
|
|
/*!
|
|
*
|
|
* rotation of a vector by axis & angle
|
|
*
|
|
* Example:
|
|
* CAngleAxis aa(3.142f,0,1,0);
|
|
* Vec3 v(33,44,55);
|
|
* Vec3 result = aa*v;
|
|
*/
|
|
template<class F>
|
|
ILINE const Vec3_tpl<F> AngleAxis_tpl<F>::operator * ( const Vec3_tpl<F> &v ) const {
|
|
Vec3_tpl<F> origin = axis*(axis|v);
|
|
return origin + (v-origin)*cos_tpl(angle) + (axis % v)*sin_tpl(angle);
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//////////////////////////////////////////////////////////////////////
|
|
struct Plane
|
|
{
|
|
|
|
//plane-equation: n.x*x+n.y*y+n.z*z+d>0 is in front of the plane
|
|
Vec3 n; //!< normal
|
|
f32 d; //!< distance
|
|
|
|
//----------------------------------------------------------------
|
|
|
|
Plane() { };
|
|
|
|
ILINE Plane( const Plane &p ) { n=p.n; d=p.d; }
|
|
ILINE Plane( const Vec3 &normal, const f32 &distance ) { n=normal; d=distance; }
|
|
|
|
//! set normal and dist for this plane and then calculate plane type
|
|
ILINE void Set(const Vec3 &vNormal,const f32 fDist) {
|
|
n = vNormal;
|
|
d = fDist;
|
|
}
|
|
|
|
ILINE void SetPlane( const Vec3 &normal, const Vec3 &point ) {
|
|
n=normal;
|
|
d=-(point | normal);
|
|
}
|
|
|
|
ILINE friend Plane GetPlane( const Vec3 &normal, const Vec3 &point ) {
|
|
return Plane( normal,-(point|normal) );
|
|
}
|
|
|
|
/*!
|
|
*
|
|
* Constructs the plane by tree given Vec3s (=triangle)
|
|
*
|
|
* Example 1:
|
|
* \code
|
|
* Vec3 v0(1,2,3),v1(4,5,6),v2(6,5,6);
|
|
* Plane plane;
|
|
* plane.CalculatePlane(v0,v1,v2);
|
|
* \endcode
|
|
*
|
|
* Example 2:
|
|
* \code
|
|
* Vec3 v0(1,2,3),v1(4,5,6),v2(6,5,6);
|
|
* Plane plane=CalculatePlane(v0,v1,v2);
|
|
* \endcode
|
|
*
|
|
*/
|
|
ILINE void SetPlane( const Vec3 &v0, const Vec3 &v1, const Vec3 &v2 ) {
|
|
n = GetNormalized((v1-v0)%(v0-v2)); //vector cross-product
|
|
d = -(n | v0); //calculate d-value
|
|
}
|
|
ILINE friend Plane GetPlane( const Vec3 &v0, const Vec3 &v1, const Vec3 &v2 ) {
|
|
Plane p;
|
|
p.n = GetNormalized((v1-v0)%(v0-v2)); //vector cross-product
|
|
p.d = -(p.n | v0); //calculate d-value
|
|
return p;
|
|
}
|
|
|
|
/*!
|
|
*
|
|
* Computes signed distance from point to plane.
|
|
* This is the standart plane-equation: d=Ax*By*Cz+D.
|
|
* The normal-vector is assumed to be normalized.
|
|
*
|
|
* Example:
|
|
* Vec3 v(1,2,3);
|
|
* Plane plane=CalculatePlane(v0,v1,v2);
|
|
* f32 distance = plane|v;
|
|
*
|
|
*/
|
|
ILINE f32 operator | ( const Vec3 &point ) const { return (n | point) + d; }
|
|
|
|
|
|
//! check for equality between two planes
|
|
ILINE friend bool operator ==(const Plane &p1, const Plane &p2) {
|
|
if (fabsf(p1.n.x-p2.n.x)>0.0001f) return (false);
|
|
if (fabsf(p1.n.y-p2.n.y)>0.0001f) return (false);
|
|
if (fabsf(p1.n.z-p2.n.z)>0.0001f) return (false);
|
|
if (fabsf(p1.d-p2.d)<0.01f) return(true);
|
|
return (false);
|
|
}
|
|
|
|
|
|
//-------------------------------------------------------------------------------------
|
|
//---------old stuff-------------------------------------------------------------------
|
|
//---------dont use, many bugs, danger danger -----------------------------------------
|
|
//-------------------------------------------------------------------------------------
|
|
|
|
//! calc the plane giving 3 points
|
|
//DANGER, DANGER! calculation of d-value is wrong
|
|
void CalcPlane(const Vec3 &p0, const Vec3 &p1, const Vec3 &p2) {
|
|
n = (p0-p1)^(p2-p1);
|
|
n.Normalize();
|
|
d = (p0*n);
|
|
}
|
|
|
|
|
|
|
|
//! Makes the plane by 3 given points
|
|
void Init(const Vec3 & v1, const Vec3 & v2, const Vec3 & v3)
|
|
{
|
|
Vec3 u, v, t;
|
|
u = v2 - v1;
|
|
v = v2 - v3;
|
|
t = u ^ v;
|
|
t.Normalize();
|
|
n = t;
|
|
d = t*v1;
|
|
}
|
|
|
|
//! same as above for const members
|
|
ILINE f32 DistFromPlane(const Vec3 &vPoint) const {
|
|
return (n*vPoint-d);
|
|
}
|
|
|
|
Vec3 MirrorVector(Vec3& i) {
|
|
return i - n*((n|i)*2);
|
|
}
|
|
|
|
Vec3 MirrorPosition(Vec3& i) {
|
|
return i - n * (2.f * ((n|i) - d));
|
|
}
|
|
|
|
};
|
|
|
|
|
|
|
|
//! calculate 2 vector that form a orthogonal base with a given input vector (by M.M.)
|
|
//! /param invDirection input direction (has to be normalized)
|
|
//! /param outvA first output vector that is perpendicular to the input direction
|
|
//! /param outvB second output vector that is perpendicular the input vector and the first output vector
|
|
ILINE void GetOtherBaseVec( const Vec3 &invDirection, Vec3 &outvA, Vec3 &outvB )
|
|
{
|
|
if(invDirection.z<-0.5f || invDirection.z>0.5f)
|
|
{
|
|
outvA.x=invDirection.z;
|
|
outvA.y=invDirection.y;
|
|
outvA.z=-invDirection.x;
|
|
}
|
|
else
|
|
{
|
|
outvA.x=invDirection.y;
|
|
outvA.y=-invDirection.x;
|
|
outvA.z=invDirection.z;
|
|
}
|
|
outvB = invDirection.Cross(outvA);
|
|
outvB.Normalize();
|
|
outvA = invDirection.Cross(outvB);
|
|
|
|
// without this normalization the results are not good enouth (Cross product introduce a errors)
|
|
outvA.Normalize();
|
|
}
|
|
|
|
|
|
|
|
#endif //vector
|