mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-14 22:00:05 +00:00
1184 lines
33 KiB
C++
1184 lines
33 KiB
C++
//
|
|
//
|
|
// Typical 3d vector math code.
|
|
// By S Melax 1998-2008
|
|
//
|
|
//
|
|
//
|
|
|
|
#include "vecmath.h"
|
|
#include <memory.h> // for memcpy
|
|
#include <float.h>
|
|
|
|
float squared(float a){return a*a;}
|
|
float clamp(float a,const float minval, const float maxval) {return Min(maxval,Max(minval,a));}
|
|
int clamp(int a,const int minval, const int maxval) {return Min(maxval,Max(minval,a));}
|
|
|
|
|
|
float Round(float a,float precision)
|
|
{
|
|
return floorf(0.5f+a/precision)*precision;
|
|
}
|
|
|
|
|
|
float Interpolate(const float &f0,const float &f1,float alpha)
|
|
{
|
|
return f0*(1-alpha) + f1*alpha;
|
|
}
|
|
|
|
|
|
int argmin(const float a[],int n)
|
|
{
|
|
int r=0;
|
|
for(int i=1;i<n;i++)
|
|
{
|
|
if(a[i]<a[r])
|
|
{
|
|
r = i;
|
|
}
|
|
}
|
|
return r;
|
|
}
|
|
|
|
|
|
int argmax(const float a[],int n)
|
|
{
|
|
int r=0;
|
|
for(int i=1;i<n;i++)
|
|
{
|
|
if(a[i]>a[r])
|
|
{
|
|
r = i;
|
|
}
|
|
}
|
|
return r;
|
|
}
|
|
|
|
|
|
|
|
//------------ float3 (3D) --------------
|
|
|
|
|
|
|
|
float3 vabs(const float3 &v)
|
|
{
|
|
return float3(fabsf(v.x),fabsf(v.y),fabsf(v.z));
|
|
}
|
|
|
|
|
|
|
|
float3 safenormalize(const float3 &v)
|
|
{
|
|
if(magnitude(v)<=0.0f)
|
|
{
|
|
return float3(1,0,0);
|
|
}
|
|
return normalize(v);
|
|
}
|
|
|
|
float3 Round(const float3 &a,float precision)
|
|
{
|
|
return float3(Round(a.x,precision),Round(a.y,precision),Round(a.z,precision));
|
|
}
|
|
|
|
|
|
float3 Interpolate(const float3 &v0,const float3 &v1,float alpha)
|
|
{
|
|
return v0*(1-alpha) + v1*alpha;
|
|
}
|
|
|
|
float3 Orth(const float3& v)
|
|
{
|
|
float3 absv=vabs(v);
|
|
float3 u(1,1,1);
|
|
u[argmax(&absv[0],3)] =0.0f;
|
|
return normalize(cross(u,v));
|
|
}
|
|
|
|
void BoxLimits(const float3 *verts,int verts_count, float3 &bmin,float3 &bmax)
|
|
{
|
|
bmin=float3( FLT_MAX, FLT_MAX, FLT_MAX);
|
|
bmax=float3(-FLT_MAX,-FLT_MAX,-FLT_MAX);
|
|
for(int i=0;i<verts_count;i++)
|
|
{
|
|
bmin = VectorMin(bmin,verts[i]);
|
|
bmax = VectorMax(bmax,verts[i]);
|
|
}
|
|
}
|
|
void BoxLimits(const float4 *verts,int verts_count, float3 &bmin,float3 &bmax)
|
|
{
|
|
bmin=float3( FLT_MAX, FLT_MAX, FLT_MAX);
|
|
bmax=float3(-FLT_MAX,-FLT_MAX,-FLT_MAX);
|
|
for(int i=0;i<verts_count;i++)
|
|
{
|
|
bmin = VectorMin(bmin,verts[i].xyz());
|
|
bmax = VectorMax(bmax,verts[i].xyz());
|
|
}
|
|
}
|
|
int overlap(const float3 &bmina,const float3 &bmaxa,const float3 &bminb,const float3 &bmaxb)
|
|
{
|
|
for(int j=0;j<3;j++)
|
|
{
|
|
if(bmina[j]>bmaxb[j]) return 0;
|
|
if(bminb[j]>bmaxa[j]) return 0;
|
|
}
|
|
return 1;
|
|
}
|
|
|
|
// the statement v1*v2 is ambiguous since there are 3 types
|
|
// of vector multiplication
|
|
// - componantwise (for example combining colors)
|
|
// - dot product
|
|
// - cross product
|
|
// Therefore we never declare/implement this function.
|
|
// So we will never see: float3 operator*(float3 a,float3 b)
|
|
|
|
|
|
|
|
|
|
//------------ float3x3 ---------------
|
|
float Determinant(const float3x3 &m)
|
|
{
|
|
return m.x.x*m.y.y*m.z.z + m.y.x*m.z.y*m.x.z + m.z.x*m.x.y*m.y.z
|
|
-m.x.x*m.z.y*m.y.z - m.y.x*m.x.y*m.z.z - m.z.x*m.y.y*m.x.z ;
|
|
}
|
|
|
|
float3x3 Inverse(const float3x3 &a)
|
|
{
|
|
float3x3 b;
|
|
float d=Determinant(a);
|
|
assert(d!=0);
|
|
for(int i=0;i<3;i++)
|
|
{
|
|
for(int j=0;j<3;j++)
|
|
{
|
|
int i1=(i+1)%3;
|
|
int i2=(i+2)%3;
|
|
int j1=(j+1)%3;
|
|
int j2=(j+2)%3;
|
|
// reverse indexs i&j to take transpose
|
|
b[j][i] = (a[i1][j1]*a[i2][j2]-a[i1][j2]*a[i2][j1])/d;
|
|
}
|
|
}
|
|
// Matrix check=a*b; // Matrix 'check' should be the identity (or close to it)
|
|
return b;
|
|
}
|
|
|
|
|
|
float3x3 Transpose( const float3x3& m )
|
|
{
|
|
return float3x3( float3(m.x.x,m.y.x,m.z.x),
|
|
float3(m.x.y,m.y.y,m.z.y),
|
|
float3(m.x.z,m.y.z,m.z.z));
|
|
}
|
|
|
|
|
|
float3 operator*(const float3& v , const float3x3 &m ) {
|
|
return float3((m.x.x*v.x + m.y.x*v.y + m.z.x*v.z),
|
|
(m.x.y*v.x + m.y.y*v.y + m.z.y*v.z),
|
|
(m.x.z*v.x + m.y.z*v.y + m.z.z*v.z));
|
|
}
|
|
float3 operator*(const float3x3 &m,const float3& v ) {
|
|
return float3(dot(m.x,v),dot(m.y,v),dot(m.z,v));
|
|
}
|
|
|
|
|
|
float3x3 operator*( const float3x3& a, const float3x3& b )
|
|
{
|
|
return float3x3(a.x*b,a.y*b,a.z*b);
|
|
}
|
|
|
|
float3x3 operator*( const float3x3& a, const float& s )
|
|
{
|
|
return float3x3(a.x*s, a.y*s ,a.z*s);
|
|
}
|
|
float3x3 operator/( const float3x3& a, const float& s )
|
|
{
|
|
float t=1/s;
|
|
return float3x3(a.x*t, a.y*t ,a.z*t);
|
|
}
|
|
float3x3 operator+( const float3x3& a, const float3x3& b )
|
|
{
|
|
return float3x3(a.x+b.x, a.y+b.y, a.z+b.z);
|
|
}
|
|
float3x3 operator-( const float3x3& a, const float3x3& b )
|
|
{
|
|
return float3x3(a.x-b.x, a.y-b.y, a.z-b.z);
|
|
}
|
|
float3x3 &operator+=( float3x3& a, const float3x3& b )
|
|
{
|
|
a.x+=b.x;
|
|
a.y+=b.y;
|
|
a.z+=b.z;
|
|
return a;
|
|
}
|
|
float3x3 &operator-=( float3x3& a, const float3x3& b )
|
|
{
|
|
a.x-=b.x;
|
|
a.y-=b.y;
|
|
a.z-=b.z;
|
|
return a;
|
|
}
|
|
float3x3 &operator*=( float3x3& a, const float& s )
|
|
{
|
|
a.x*=s;
|
|
a.y*=s;
|
|
a.z*=s;
|
|
return a;
|
|
}
|
|
|
|
float3x3 outerprod(const float3& a,const float3& b)
|
|
{
|
|
return float3x3(a.x*b,a.y*b,a.z*b); // a is a column vector b is a row vector
|
|
}
|
|
|
|
//--------------- 4D ----------------
|
|
|
|
float4 operator*( const float4& v, const float4x4& m )
|
|
{
|
|
return v.x*m.x + v.y*m.y + v.z*m.z + v.w*m.w; // yes this actually works
|
|
}
|
|
|
|
|
|
// Dont implement m*v for now, since that might confuse us
|
|
// All our transforms are based on multiplying the "row" vector on the left
|
|
//float4 operator*(const float4x4& m , const float4& v )
|
|
//{
|
|
// return float4(dot(v,m.x),dot(v,m.y),dot(v,m.z),dot(v,m.w));
|
|
//}
|
|
|
|
|
|
float4x4 operator*( const float4x4& a, const float4x4& b )
|
|
{
|
|
return float4x4(a.x*b,a.y*b,a.z*b,a.w*b);
|
|
}
|
|
|
|
float4x4 MatrixTranspose(const float4x4 &m)
|
|
{
|
|
return float4x4(
|
|
m.x.x, m.y.x, m.z.x, m.w.x,
|
|
m.x.y, m.y.y, m.z.y, m.w.y,
|
|
m.x.z, m.y.z, m.z.z, m.w.z,
|
|
m.x.w, m.y.w, m.z.w, m.w.w );
|
|
}
|
|
|
|
float4x4 MatrixRigidInverse(const float4x4 &m)
|
|
{
|
|
float4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
|
|
float4x4 rot = m;
|
|
rot.w = float4(0,0,0,1);
|
|
return trans_inverse * MatrixTranspose(rot);
|
|
}
|
|
|
|
|
|
float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf )
|
|
{
|
|
float h = 1.0f/tanf(fovy/2.0f); // view space height
|
|
float w = h / aspect ; // view space width
|
|
return float4x4(
|
|
w, 0, 0 , 0,
|
|
0, h, 0 , 0,
|
|
0, 0, zf/(zn-zf) , -1,
|
|
0, 0, zn*zf/(zn-zf) , 0 );
|
|
}
|
|
|
|
|
|
|
|
float4x4 MatrixLookAt(const float3& eye, const float3& at, const float3& up)
|
|
{
|
|
float4x4 m;
|
|
m.w.w = 1.0f;
|
|
m.w.xyz() = eye;
|
|
m.z.xyz() = normalize(eye-at);
|
|
m.x.xyz() = normalize(cross(up,m.z.xyz()));
|
|
m.y.xyz() = cross(m.z.xyz(),m.x.xyz());
|
|
return MatrixRigidInverse(m);
|
|
}
|
|
|
|
|
|
float4x4 MatrixTranslation(const float3 &t)
|
|
{
|
|
return float4x4(
|
|
1, 0, 0, 0,
|
|
0, 1, 0, 0,
|
|
0, 0, 1, 0,
|
|
t.x,t.y,t.z,1 );
|
|
}
|
|
|
|
|
|
float4x4 MatrixRotationZ(const float angle_radians)
|
|
{
|
|
float s = sinf(angle_radians);
|
|
float c = cosf(angle_radians);
|
|
return float4x4(
|
|
c, s, 0, 0,
|
|
-s, c, 0, 0,
|
|
0, 0, 1, 0,
|
|
0, 0, 0, 1 );
|
|
}
|
|
|
|
|
|
|
|
int operator==( const float4x4 &a, const float4x4 &b )
|
|
{
|
|
return (a.x==b.x && a.y==b.y && a.z==b.z && a.w==b.w);
|
|
}
|
|
|
|
|
|
float4x4 Inverse(const float4x4 &m)
|
|
{
|
|
float4x4 d;
|
|
float *dst = &d.x.x;
|
|
float tmp[12]; /* temp array for pairs */
|
|
float src[16]; /* array of transpose source matrix */
|
|
float det; /* determinant */
|
|
/* transpose matrix */
|
|
for ( int i = 0; i < 4; i++) {
|
|
src[i] = m(i,0) ;
|
|
src[i + 4] = m(i,1);
|
|
src[i + 8] = m(i,2);
|
|
src[i + 12] = m(i,3);
|
|
}
|
|
/* calculate pairs for first 8 elements (cofactors) */
|
|
tmp[0] = src[10] * src[15];
|
|
tmp[1] = src[11] * src[14];
|
|
tmp[2] = src[9] * src[15];
|
|
tmp[3] = src[11] * src[13];
|
|
tmp[4] = src[9] * src[14];
|
|
tmp[5] = src[10] * src[13];
|
|
tmp[6] = src[8] * src[15];
|
|
tmp[7] = src[11] * src[12];
|
|
tmp[8] = src[8] * src[14];
|
|
tmp[9] = src[10] * src[12];
|
|
tmp[10] = src[8] * src[13];
|
|
tmp[11] = src[9] * src[12];
|
|
/* calculate first 8 elements (cofactors) */
|
|
dst[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
|
|
dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
|
|
dst[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
|
|
dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
|
|
dst[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
|
|
dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
|
|
dst[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
|
|
dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
|
|
dst[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
|
|
dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
|
|
dst[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
|
|
dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
|
|
dst[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
|
|
dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
|
|
dst[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
|
|
dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
|
|
/* calculate pairs for second 8 elements (cofactors) */
|
|
tmp[0] = src[2]*src[7];
|
|
tmp[1] = src[3]*src[6];
|
|
tmp[2] = src[1]*src[7];
|
|
tmp[3] = src[3]*src[5];
|
|
tmp[4] = src[1]*src[6];
|
|
tmp[5] = src[2]*src[5];
|
|
tmp[6] = src[0]*src[7];
|
|
tmp[7] = src[3]*src[4];
|
|
tmp[8] = src[0]*src[6];
|
|
tmp[9] = src[2]*src[4];
|
|
tmp[10] = src[0]*src[5];
|
|
tmp[11] = src[1]*src[4];
|
|
/* calculate second 8 elements (cofactors) */
|
|
dst[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
|
|
dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
|
|
dst[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
|
|
dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
|
|
dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
|
|
dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
|
|
dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
|
|
dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
|
|
dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
|
|
dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
|
|
dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
|
|
dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
|
|
dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
|
|
dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
|
|
dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
|
|
dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
|
|
/* calculate determinant */
|
|
det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
|
|
/* calculate matrix inverse */
|
|
det = 1/det;
|
|
for ( int j = 0; j < 16; j++)
|
|
dst[j] *= det;
|
|
return d;
|
|
}
|
|
|
|
|
|
//--------- Quaternion --------------
|
|
|
|
template<> void Quaternion::Normalize()
|
|
{
|
|
float m = sqrtf(squared(w)+squared(x)+squared(y)+squared(z));
|
|
if(m<0.000000001f) {
|
|
w=1.0f;
|
|
x=y=z=0.0f;
|
|
return;
|
|
}
|
|
(*this) *= (1.0f/m);
|
|
}
|
|
|
|
float3 rotate( const Quaternion& q, const float3& v )
|
|
{
|
|
// The following is equivalent to:
|
|
//return (q.getmatrix() * v);
|
|
float qx2 = q.x*q.x;
|
|
float qy2 = q.y*q.y;
|
|
float qz2 = q.z*q.z;
|
|
|
|
float qxqy = q.x*q.y;
|
|
float qxqz = q.x*q.z;
|
|
float qxqw = q.x*q.w;
|
|
float qyqz = q.y*q.z;
|
|
float qyqw = q.y*q.w;
|
|
float qzqw = q.z*q.w;
|
|
return float3(
|
|
(1-2*(qy2+qz2))*v.x + (2*(qxqy-qzqw))*v.y + (2*(qxqz+qyqw))*v.z ,
|
|
(2*(qxqy+qzqw))*v.x + (1-2*(qx2+qz2))*v.y + (2*(qyqz-qxqw))*v.z ,
|
|
(2*(qxqz-qyqw))*v.x + (2*(qyqz+qxqw))*v.y + (1-2*(qx2+qy2))*v.z );
|
|
}
|
|
|
|
|
|
Quaternion slerp(const Quaternion &_a, const Quaternion& b, float interp )
|
|
{
|
|
Quaternion a=_a;
|
|
if(dot(a,b) <0.0)
|
|
{
|
|
a.w=-a.w;
|
|
a.x=-a.x;
|
|
a.y=-a.y;
|
|
a.z=-a.z;
|
|
}
|
|
float d = dot(a,b);
|
|
if(d>=1.0) {
|
|
return a;
|
|
}
|
|
float theta = acosf(d);
|
|
if(theta==0.0f) { return(a);}
|
|
return a*(sinf(theta-interp*theta)/sinf(theta)) + b*(sinf(interp*theta)/sinf(theta));
|
|
}
|
|
|
|
|
|
Quaternion Interpolate(const Quaternion &q0,const Quaternion &q1,float alpha) {
|
|
return slerp(q0,q1,alpha);
|
|
}
|
|
|
|
|
|
Quaternion YawPitchRoll( float yaw, float pitch, float roll )
|
|
{
|
|
return QuatFromAxisAngle(float3(0.0f,0.0f,1.0f),DegToRad(yaw ))
|
|
* QuatFromAxisAngle(float3(1.0f,0.0f,0.0f),DegToRad(pitch))
|
|
* QuatFromAxisAngle(float3(0.0f,1.0f,0.0f),DegToRad(roll ));
|
|
}
|
|
|
|
float Yaw( const Quaternion& q )
|
|
{
|
|
static float3 v;
|
|
v=q.ydir();
|
|
return (v.y==0.0&&v.x==0.0) ? 0.0f: RadToDeg(atan2f(-v.x,v.y));
|
|
}
|
|
|
|
float Pitch( const Quaternion& q )
|
|
{
|
|
static float3 v;
|
|
v=q.ydir();
|
|
return RadToDeg(atan2f(v.z,sqrtf(squared(v.x)+squared(v.y))));
|
|
}
|
|
|
|
float Roll( const Quaternion &_q )
|
|
{
|
|
Quaternion q=_q;
|
|
q = QuatFromAxisAngle(float3(0.0f,0.0f,1.0f),-DegToRad(Yaw(q))) *q;
|
|
q = QuatFromAxisAngle(float3(1.0f,0.0f,0.0f),-DegToRad(Pitch(q))) *q;
|
|
return RadToDeg(atan2f(-q.xdir().z,q.xdir().x));
|
|
}
|
|
|
|
float Yaw( const float3& v )
|
|
{
|
|
return (v.y==0.0&&v.x==0.0) ? 0.0f: RadToDeg(atan2f(-v.x,v.y));
|
|
}
|
|
|
|
float Pitch( const float3& v )
|
|
{
|
|
return RadToDeg(atan2f(v.z,sqrtf(squared(v.x)+squared(v.y))));
|
|
}
|
|
|
|
|
|
|
|
|
|
|
|
|
|
//--------- utility functions -------------
|
|
|
|
// RotationArc()
|
|
// Given two vectors v0 and v1 this function
|
|
// returns quaternion q where q*v0==v1.
|
|
// Routine taken from game programming gems.
|
|
Quaternion RotationArc(float3 v0,float3 v1){
|
|
static Quaternion q;
|
|
v0 = normalize(v0); // Comment these two lines out if you know its not needed.
|
|
v1 = normalize(v1); // If vector is already unit length then why do it again?
|
|
float3 c = cross(v0,v1);
|
|
float d = dot(v0,v1);
|
|
if(d<=-1.0f) { float3 a=Orth(v0); return Quaternion(a.x,a.y,a.z,0);} // 180 about any orthogonal axis axis
|
|
float s = sqrtf((1+d)*2);
|
|
q.x = c.x / s;
|
|
q.y = c.y / s;
|
|
q.z = c.z / s;
|
|
q.w = s /2.0f;
|
|
return q;
|
|
}
|
|
|
|
|
|
float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v)
|
|
{
|
|
// builds a 4x4 transformation matrix based on orientation q and translation v
|
|
float qx2 = q.x*q.x;
|
|
float qy2 = q.y*q.y;
|
|
float qz2 = q.z*q.z;
|
|
|
|
float qxqy = q.x*q.y;
|
|
float qxqz = q.x*q.z;
|
|
float qxqw = q.x*q.w;
|
|
float qyqz = q.y*q.z;
|
|
float qyqw = q.y*q.w;
|
|
float qzqw = q.z*q.w;
|
|
|
|
return float4x4(
|
|
1-2*(qy2+qz2),
|
|
2*(qxqy+qzqw),
|
|
2*(qxqz-qyqw),
|
|
0 ,
|
|
2*(qxqy-qzqw),
|
|
1-2*(qx2+qz2),
|
|
2*(qyqz+qxqw),
|
|
0 ,
|
|
2*(qxqz+qyqw),
|
|
2*(qyqz-qxqw),
|
|
1-2*(qx2+qy2),
|
|
0 ,
|
|
v.x ,
|
|
v.y ,
|
|
v.z ,
|
|
1.0f );
|
|
}
|
|
|
|
|
|
|
|
float3 PlaneLineIntersection(const float3 &normal,const float dist, const float3 &p0, const float3 &p1)
|
|
{
|
|
// returns the point where the line p0-p1 intersects the plane n&d
|
|
float3 dif;
|
|
dif = p1-p0;
|
|
float dn= dot(normal,dif);
|
|
float t = -(dist+dot(normal,p0) )/dn;
|
|
return p0 + (dif*t);
|
|
}
|
|
|
|
float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a)
|
|
{
|
|
// project point a on segment [p0,p1]
|
|
float3 d= p1-p0;
|
|
float t= dot(d,(a-p0)) / dot(d,d);
|
|
return p0+ d*t;
|
|
}
|
|
|
|
|
|
float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a)
|
|
{
|
|
// project point a on segment [p0,p1]
|
|
float3 d= p1-p0;
|
|
float t= dot(d,(a-p0)) / dot(d,d);
|
|
return t;
|
|
}
|
|
|
|
|
|
|
|
float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2)
|
|
{
|
|
// return the normal of the triangle
|
|
// inscribed by v0, v1, and v2
|
|
float3 cp=cross(v1-v0,v2-v1);
|
|
float m=magnitude(cp);
|
|
if(m==0) return float3(1,0,0);
|
|
return cp*(1.0f/m);
|
|
}
|
|
|
|
|
|
|
|
int BoxInside(const float3 &p, const float3 &bmin, const float3 &bmax)
|
|
{
|
|
return (p.x >= bmin.x && p.x <=bmax.x &&
|
|
p.y >= bmin.y && p.y <=bmax.y &&
|
|
p.z >= bmin.z && p.z <=bmax.z );
|
|
}
|
|
|
|
|
|
int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax,float3 *impact)
|
|
{
|
|
if(BoxInside(v0,bmin,bmax))
|
|
{
|
|
*impact=v0;
|
|
return 1;
|
|
}
|
|
if(v0.x<=bmin.x && v1.x>=bmin.x)
|
|
{
|
|
float a = (bmin.x-v0.x)/(v1.x-v0.x);
|
|
//v.x = bmin.x;
|
|
float vy = (1-a) *v0.y + a*v1.y;
|
|
float vz = (1-a) *v0.z + a*v1.z;
|
|
if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z)
|
|
{
|
|
impact->x = bmin.x;
|
|
impact->y = vy;
|
|
impact->z = vz;
|
|
return 1;
|
|
}
|
|
}
|
|
else if(v0.x >= bmax.x && v1.x <= bmax.x)
|
|
{
|
|
float a = (bmax.x-v0.x)/(v1.x-v0.x);
|
|
//v.x = bmax.x;
|
|
float vy = (1-a) *v0.y + a*v1.y;
|
|
float vz = (1-a) *v0.z + a*v1.z;
|
|
if(vy>=bmin.y && vy<=bmax.y && vz>=bmin.z && vz<=bmax.z)
|
|
{
|
|
impact->x = bmax.x;
|
|
impact->y = vy;
|
|
impact->z = vz;
|
|
return 1;
|
|
}
|
|
}
|
|
if(v0.y<=bmin.y && v1.y>=bmin.y)
|
|
{
|
|
float a = (bmin.y-v0.y)/(v1.y-v0.y);
|
|
float vx = (1-a) *v0.x + a*v1.x;
|
|
//v.y = bmin.y;
|
|
float vz = (1-a) *v0.z + a*v1.z;
|
|
if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z)
|
|
{
|
|
impact->x = vx;
|
|
impact->y = bmin.y;
|
|
impact->z = vz;
|
|
return 1;
|
|
}
|
|
}
|
|
else if(v0.y >= bmax.y && v1.y <= bmax.y)
|
|
{
|
|
float a = (bmax.y-v0.y)/(v1.y-v0.y);
|
|
float vx = (1-a) *v0.x + a*v1.x;
|
|
// vy = bmax.y;
|
|
float vz = (1-a) *v0.z + a*v1.z;
|
|
if(vx>=bmin.x && vx<=bmax.x && vz>=bmin.z && vz<=bmax.z)
|
|
{
|
|
impact->x = vx;
|
|
impact->y = bmax.y;
|
|
impact->z = vz;
|
|
return 1;
|
|
}
|
|
}
|
|
if(v0.z<=bmin.z && v1.z>=bmin.z)
|
|
{
|
|
float a = (bmin.z-v0.z)/(v1.z-v0.z);
|
|
float vx = (1-a) *v0.x + a*v1.x;
|
|
float vy = (1-a) *v0.y + a*v1.y;
|
|
// v.z = bmin.z;
|
|
if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x)
|
|
{
|
|
impact->x = vx;
|
|
impact->y = vy;
|
|
impact->z = bmin.z;
|
|
return 1;
|
|
}
|
|
}
|
|
else if(v0.z >= bmax.z && v1.z <= bmax.z)
|
|
{
|
|
float a = (bmax.z-v0.z)/(v1.z-v0.z);
|
|
float vx = (1-a) *v0.x + a*v1.x;
|
|
float vy = (1-a) *v0.y + a*v1.y;
|
|
// v.z = bmax.z;
|
|
if(vy>=bmin.y && vy<=bmax.y && vx>=bmin.x && vx<=bmax.x)
|
|
{
|
|
impact->x = vx;
|
|
impact->y = vy;
|
|
impact->z = bmax.z;
|
|
return 1;
|
|
}
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
|
|
float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint, float3 *vpoint)
|
|
{
|
|
static float3 cp;
|
|
cp = normalize(cross(udir,vdir));
|
|
|
|
float distu = -dot(cp,ustart);
|
|
float distv = -dot(cp,vstart);
|
|
float dist = (float)fabs(distu-distv);
|
|
if(upoint)
|
|
{
|
|
float3 normal = normalize(cross(vdir,cp));
|
|
*upoint = PlaneLineIntersection(normal,-dot(normal,vstart),ustart,ustart+udir);
|
|
}
|
|
if(vpoint)
|
|
{
|
|
float3 normal = normalize(cross(udir,cp));
|
|
*vpoint = PlaneLineIntersection(normal,-dot(normal,ustart),vstart,vstart+vdir);
|
|
}
|
|
return dist;
|
|
}
|
|
|
|
|
|
Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir1, const float3 &dir2)
|
|
{
|
|
// routine taken from game programming gems.
|
|
// Implement track ball functionality to spin stuf on the screen
|
|
// cop center of projection
|
|
// cor center of rotation
|
|
// dir1 old mouse direction
|
|
// dir2 new mouse direction
|
|
// pretend there is a sphere around cor. Then find the points
|
|
// where dir1 and dir2 intersect that sphere. Find the
|
|
// rotation that takes the first point to the second.
|
|
float m;
|
|
// compute plane
|
|
float3 nrml = cor - cop;
|
|
float fudgefactor = 1.0f/(magnitude(nrml) * 0.25f); // since trackball proportional to distance from cop
|
|
nrml = normalize(nrml);
|
|
float dist = -dot(nrml,cor);
|
|
float3 u= PlaneLineIntersection(nrml,dist,cop,cop+dir1);
|
|
u=u-cor;
|
|
u=u*fudgefactor;
|
|
m= magnitude(u);
|
|
if(m>1)
|
|
{
|
|
u/=m;
|
|
}
|
|
else
|
|
{
|
|
u=u - (nrml * sqrtf(1-m*m));
|
|
}
|
|
float3 v= PlaneLineIntersection(nrml,dist,cop,cop+dir2);
|
|
v=v-cor;
|
|
v=v*fudgefactor;
|
|
m= magnitude(v);
|
|
if(m>1)
|
|
{
|
|
v/=m;
|
|
}
|
|
else
|
|
{
|
|
v=v - (nrml * sqrtf(1-m*m));
|
|
}
|
|
return RotationArc(u,v);
|
|
}
|
|
|
|
|
|
int countpolyhit=0;
|
|
int HitCheckPoly(const float3 *vert, const int n, const float3 &v0, const float3 &v1, float3 *impact, float3 *normal)
|
|
{
|
|
countpolyhit++;
|
|
int i;
|
|
float3 nrml(0,0,0);
|
|
for(i=0;i<n;i++)
|
|
{
|
|
int i1=(i+1)%n;
|
|
int i2=(i+2)%n;
|
|
nrml = nrml + cross(vert[i1]-vert[i],vert[i2]-vert[i1]);
|
|
}
|
|
|
|
float m = magnitude(nrml);
|
|
if(m==0.0)
|
|
{
|
|
return 0;
|
|
}
|
|
nrml = nrml * (1.0f/m);
|
|
float dist = -dot(nrml,vert[0]);
|
|
float d0,d1;
|
|
if((d0=dot(v0,nrml)+dist) <0 || (d1=dot(v1,nrml)+dist) >0)
|
|
{
|
|
return 0;
|
|
}
|
|
|
|
static float3 the_point;
|
|
// By using the cached plane distances d0 and d1
|
|
// we can optimize the following:
|
|
// the_point = planelineintersection(nrml,dist,v0,v1);
|
|
float a = d0/(d0-d1);
|
|
the_point = v0*(1-a) + v1*a;
|
|
|
|
|
|
int inside=1;
|
|
for(int j=0;inside && j<n;j++)
|
|
{
|
|
// let inside = 0 if outside
|
|
float3 pp1,pp2,side;
|
|
pp1 = vert[j] ;
|
|
pp2 = vert[(j+1)%n];
|
|
side = cross((pp2-pp1),(the_point-pp1));
|
|
inside = (dot(nrml,side) >= 0.0);
|
|
}
|
|
if(inside)
|
|
{
|
|
if(normal){*normal=nrml;}
|
|
if(impact){*impact=the_point;}
|
|
}
|
|
return inside;
|
|
}
|
|
|
|
int SolveQuadratic(float a,float b,float c,float *ta,float *tb) // if true returns roots ta,tb where ta<=tb
|
|
{
|
|
assert(ta);
|
|
assert(tb);
|
|
float d = b*b-4.0f*a*c; // discriminant
|
|
if(d<0.0f) return 0;
|
|
float sqd = sqrtf(d);
|
|
*ta = (-b-sqd) / (2.0f * a);
|
|
*tb = (-b+sqd) / (2.0f * a);
|
|
return 1;
|
|
}
|
|
|
|
int HitCheckRaySphere(const float3& sphereposition,float radius, const float3& _v0, const float3& _v1, float3 *impact,float3 *normal)
|
|
{
|
|
assert(impact);
|
|
assert(normal);
|
|
float3 dv = _v1-_v0;
|
|
float3 v0 = _v0 - sphereposition; // solve in coord system of the sphere
|
|
if(radius<=0.0f || _v0==_v1) return 0; // only true if point moves from outside to inside sphere.
|
|
float a = dot(dv,dv);
|
|
float b = 2.0f * dot(dv,v0);
|
|
float c = dot(v0,v0) - radius*radius;
|
|
if(c<0.0f) return 0; // we are already inside the sphere.
|
|
|
|
float ta, tb;
|
|
int doesIntersect = SolveQuadratic(a, b, c, &ta, &tb);
|
|
|
|
if (!doesIntersect) return 0;
|
|
|
|
if (ta >= 0.0f && ta <= 1.0f && (ta <= tb || tb<=0.0f))
|
|
{
|
|
*impact = _v0 + dv * ta;
|
|
*normal = (v0 + dv*ta)/radius;
|
|
return 1;
|
|
}
|
|
if (tb >= 0.0f && tb <= 1.0f)
|
|
{
|
|
assert(tb <= ta || ta <=0.0f); // tb must be better than ta
|
|
*impact = _v0 + dv * tb;
|
|
*normal = (v0 + dv*tb)/radius;
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int HitCheckRayCylinder(const float3 &p0,const float3 &p1,float radius,const float3& _v0,const float3& _v1, float3 *impact,float3 *normal)
|
|
{
|
|
assert(impact);
|
|
assert(normal);
|
|
// only concerned about hitting the sides, not the caps for now
|
|
float3x3 m=RotationArc(p1-p0,float3(0,0,1.0f)).getmatrix();
|
|
float h = ((p1-p0)*m ).z;
|
|
float3 v0 = (_v0-p0) *m;
|
|
float3 v1 = (_v1-p0) *m;
|
|
if(v0.z <= 0.0f && v1.z <= 0.0f) return 0; // entirely below cylinder
|
|
if(v0.z >= h && v1.z >= h ) return 0; // ray is above cylinder
|
|
if(v0.z <0.0f ) v0 = PlaneLineIntersection(float3(0,0,1.0f), 0,v0,v1); // crop to cylinder range
|
|
if(v1.z <0.0f ) v1 = PlaneLineIntersection(float3(0,0,1.0f), 0,v0,v1);
|
|
if(v0.z > h ) v0 = PlaneLineIntersection(float3(0,0,1.0f),-h,v0,v1);
|
|
if(v1.z > h ) v1 = PlaneLineIntersection(float3(0,0,1.0f),-h,v0,v1);
|
|
if(v0.x==v1.x && v0.y==v1.y) return 0;
|
|
float3 dv = v1-v0;
|
|
|
|
float a = dv.x*dv.x+dv.y*dv.y;
|
|
float b = 2.0f * (dv.x*v0.x+dv.y*v0.y);
|
|
float c = (v0.x*v0.x+v0.y*v0.y) - radius*radius;
|
|
if(c<0.0f) return 0; // we are already inside the cylinder .
|
|
|
|
float ta, tb;
|
|
int doesIntersect = SolveQuadratic(a, b, c, &ta, &tb);
|
|
|
|
if (!doesIntersect) return 0;
|
|
|
|
if (ta >= 0.0f && ta <= 1.0f && (ta <= tb || tb<=0.0f))
|
|
{
|
|
*impact = (v0 + dv * ta)*Transpose(m) + p0;
|
|
*normal = (float3(v0.x,v0.y,0.0f) + float3(dv.x,dv.y,0) * ta) /radius * Transpose(m);
|
|
return 1;
|
|
}
|
|
if (tb >= 0.0f && tb <= 1.0f)
|
|
{
|
|
assert(tb <= ta || ta <=0.0f); // tb must be better than ta
|
|
*impact = (v0 + dv * tb)*Transpose(m) + p0; // compute intersection in original space
|
|
*normal = (float3(v0.x,v0.y,0.0f) + float3(dv.x,dv.y,0) * tb) /radius * Transpose(m);
|
|
return 1;
|
|
}
|
|
return 0;
|
|
}
|
|
|
|
int HitCheckSweptSphereTri(const float3 &p0,const float3 &p1,const float3 &p2,float radius, const float3& v0,const float3& _v1, float3 *impact,float3 *normal)
|
|
{
|
|
float3 unused;
|
|
if(!normal) normal=&unused;
|
|
float3 v1=_v1; // so we can update v1 after each sub intersection test if necessary
|
|
int hit=0;
|
|
float3 cp = cross(p1-p0,p2-p0);
|
|
if(dot(cp,v1-v0)>=0.0f) return 0; // coming from behind and/or moving away
|
|
float3 n = normalize(cp);
|
|
float3 tv[3];
|
|
tv[0] = p0 + n*radius;
|
|
tv[1] = p1 + n*radius;
|
|
tv[2] = p2 + n*radius;
|
|
hit += HitCheckPoly(tv,3,v0,v1,&v1,normal);
|
|
hit += HitCheckRayCylinder(p0,p1,radius,v0,v1,&v1,normal);
|
|
hit += HitCheckRayCylinder(p1,p2,radius,v0,v1,&v1,normal);
|
|
hit += HitCheckRayCylinder(p2,p0,radius,v0,v1,&v1,normal);
|
|
hit += HitCheckRaySphere(p0,radius,v0,v1,&v1,normal);
|
|
hit += HitCheckRaySphere(p1,radius,v0,v1,&v1,normal);
|
|
hit += HitCheckRaySphere(p2,radius,v0,v1,&v1,normal);
|
|
if(hit && impact) *impact = v1 + *normal * 0.001f;
|
|
return hit;
|
|
}
|
|
|
|
|
|
float3 PlanesIntersection(const Plane &p0,const Plane &p1, const Plane &p2)
|
|
{
|
|
float3x3 mp =Transpose(float3x3(p0.normal(),p1.normal(),p2.normal()));
|
|
float3x3 mi = Inverse(mp);
|
|
float3 b(p0.dist(),p1.dist(),p2.dist());
|
|
return -b * mi;
|
|
}
|
|
|
|
|
|
float3 PlanesIntersection(const Plane *planes,int planes_count,const float3 &seed)
|
|
{
|
|
int i;
|
|
float3x3 A; // gets initilized to 0 matrix
|
|
float3 b(0,0,0);
|
|
for(i=0;i<planes_count;i++)
|
|
{
|
|
const Plane &p=planes[i];
|
|
A += outerprod(p.normal(),p.normal());
|
|
b += p.normal() * -p.dist();
|
|
}
|
|
float3x3 evecs = Diagonalizer(A).getmatrix(); // eigenvectors
|
|
float3 evals = Diagonal(evecs*A*Transpose(evecs)); // eigenvalues
|
|
for(i=0;i<3;i++)
|
|
{
|
|
if(fabsf(evals[i])<1.0f) // not sure if they are necessarily positive
|
|
{
|
|
Plane p;
|
|
p.normal() = evecs[i]* squared(1.0f-evals[i]);
|
|
p.dist() = -dot(seed,p.normal());
|
|
A += outerprod(p.normal(),p.normal());
|
|
b += p.normal() * -p.dist();
|
|
}
|
|
}
|
|
return Inverse(A) * b;
|
|
}
|
|
|
|
|
|
Plane Transform(const Plane &p, const float3 &translation, const Quaternion &rotation)
|
|
{
|
|
// Transforms the plane by the given translation/rotation.
|
|
float3 newnormal = rotate(rotation,p.normal());
|
|
return Plane(newnormal, p.dist() - dot(newnormal,translation));
|
|
}
|
|
|
|
|
|
float3 PlaneProject(const Plane &plane, const float3 &point)
|
|
{
|
|
return point - plane.normal() * (dot(point,plane.normal())+plane.dist());
|
|
}
|
|
float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1)
|
|
{
|
|
// returns the point where the line p0-p1 intersects the plane n&d
|
|
float3 dif;
|
|
dif = p1-p0;
|
|
float dn= dot(plane.normal(),dif);
|
|
float t = -(plane.dist()+dot(plane.normal(),p0) )/dn;
|
|
return p0 + (dif*t);
|
|
}
|
|
|
|
|
|
|
|
int Clip(const float3 &plane_normal,float plane_dist,const float3 *verts_in,int count_in,float3* verts_out)
|
|
{
|
|
// clips a polygon specified by the non-indexed vertex list verts_in.
|
|
// verts_out must be preallocated with a size >= count+1
|
|
assert(verts_out);
|
|
int n=0;
|
|
int prev_status = (dot(plane_normal,verts_in[count_in-1])+plane_dist > 0) ;
|
|
for(int i=0;i<count_in;i++)
|
|
{
|
|
int status = (dot(plane_normal,verts_in[i])+plane_dist > 0) ;
|
|
if(status != prev_status)
|
|
{
|
|
verts_out[n++] = PlaneLineIntersection(plane_normal,plane_dist,verts_in[(i==0)?count_in-1:i-1],verts_in[i]);
|
|
}
|
|
if(status==0) // under
|
|
{
|
|
verts_out[n++] = verts_in[i];
|
|
}
|
|
}
|
|
assert(n<=count_in+1); // remove if intention to use this routine on convex polygons
|
|
return n;
|
|
}
|
|
|
|
int ClipPolyPoly(const float3 &normal,const float3 *clipper,int clipper_count,const float3 *verts_in, int in_count,float3 *scratch)
|
|
{
|
|
// clips polys against each other.
|
|
// requires sufficiently allocated temporary memory in scratch buffer
|
|
// function returns final number of vertices in clipped polygon.
|
|
// Resulting vertices are returned in the scratch buffer.
|
|
// if the arrays are the same &verts_in==&scratch the routine should still work anyways.
|
|
// the first argument (normal) is the normal of polygon clipper.
|
|
// its generally assumed both are convex polygons.
|
|
assert(scratch); // size should be >= 2*(clipper_count+in_count)
|
|
int i;
|
|
int bsize = clipper_count+in_count;
|
|
int count = in_count;
|
|
for(i=0;i<clipper_count;i++)
|
|
{
|
|
int i1 = (i+1)%clipper_count;
|
|
float3 n = cross(clipper[i1]-clipper[i],normal);
|
|
if(n==float3(0,0,0)) continue;
|
|
n=normalize(n);
|
|
count = Clip(n,-dot(clipper[i],n),(i==0)?verts_in:(i%2)?scratch:scratch+bsize,count,(i%2)?scratch+bsize:scratch);
|
|
assert(count<bsize);
|
|
}
|
|
if(clipper_count%2) memcpy(scratch,scratch+bsize,count*sizeof(float3));
|
|
return count;
|
|
}
|
|
|
|
float Volume(const float3 *vertices, const int3 *tris, const int count)
|
|
{
|
|
// count is the number of triangles (tris)
|
|
float volume=0;
|
|
for(int i=0; i<count; i++) // for each triangle
|
|
{
|
|
volume += Determinant(float3x3(vertices[tris[i][0]],vertices[tris[i][1]],vertices[tris[i][2]])); //divide by 6 later for efficiency
|
|
}
|
|
return volume/6.0f; // since the determinant give 6 times tetra volume
|
|
}
|
|
|
|
float3 CenterOfMass(const float3 *vertices, const int3 *tris, const int count)
|
|
{
|
|
// count is the number of triangles (tris)
|
|
float3 com(0,0,0);
|
|
float volume=0; // actually accumulates the volume*6
|
|
for(int i=0; i<count; i++) // for each triangle
|
|
{
|
|
float3x3 A(vertices[tris[i][0]],vertices[tris[i][1]],vertices[tris[i][2]]);
|
|
float vol=Determinant(A); // dont bother to divide by 6
|
|
com += vol * (A.x+A.y+A.z); // divide by 4 at end
|
|
volume+=vol;
|
|
}
|
|
com /= volume*4.0f;
|
|
return com;
|
|
}
|
|
float3x3 Inertia(const float3 *vertices, const int3 *tris, const int count, const float3& com /* =float3(0,0,0) */ )
|
|
{
|
|
// count is the number of triangles (tris)
|
|
// The moments are calculated based on the center of rotation (com) which defaults to [0,0,0] if unsupplied
|
|
// assume mass==1.0 you can multiply by mass later.
|
|
// for improved accuracy the next 3 variables, the determinant d, and its calculation should be changed to double
|
|
float volume=0; // technically this variable accumulates the volume times 6
|
|
float3 diag(0,0,0); // accumulate matrix main diagonal integrals [x*x, y*y, z*z]
|
|
float3 offd(0,0,0); // accumulate matrix off-diagonal integrals [y*z, x*z, x*y]
|
|
for(int i=0; i<count; i++) // for each triangle
|
|
{
|
|
float3x3 A(vertices[tris[i][0]]-com,vertices[tris[i][1]]-com,vertices[tris[i][2]]-com); // matrix trick for volume calc by taking determinant
|
|
float d = Determinant(A); // vol of tiny parallelapiped= d * dr * ds * dt (the 3 partials of my tetral triple integral equasion)
|
|
volume +=d; // add vol of current tetra (note it could be negative - that's ok we need that sometimes)
|
|
for(int j=0;j<3;j++)
|
|
{
|
|
int j1=(j+1)%3;
|
|
int j2=(j+2)%3;
|
|
diag[j] += (A[0][j]*A[1][j] + A[1][j]*A[2][j] + A[2][j]*A[0][j] +
|
|
A[0][j]*A[0][j] + A[1][j]*A[1][j] + A[2][j]*A[2][j] ) *d; // divide by 60.0f later;
|
|
offd[j] += (A[0][j1]*A[1][j2] + A[1][j1]*A[2][j2] + A[2][j1]*A[0][j2] +
|
|
A[0][j1]*A[2][j2] + A[1][j1]*A[0][j2] + A[2][j1]*A[1][j2] +
|
|
A[0][j1]*A[0][j2]*2+ A[1][j1]*A[1][j2]*2+ A[2][j1]*A[2][j2]*2 ) *d; // divide by 120.0f later
|
|
}
|
|
}
|
|
diag /= volume*(60.0f /6.0f); // divide by total volume (vol/6) since density=1/volume
|
|
offd /= volume*(120.0f/6.0f);
|
|
return float3x3(diag.y+diag.z , -offd.z , -offd.y,
|
|
-offd.z , diag.x+diag.z, -offd.x,
|
|
-offd.y , -offd.x , diag.x+diag.y );
|
|
}
|
|
|
|
|
|
float3x3 ShapeInertiaContrib(const float3& cor, const float3& position, const Quaternion &orientation,
|
|
const float3& shape_com, const float3x3& shape_inertia, float shape_mass)
|
|
{
|
|
// transforms 3x3 inertia tensor from local reference frame to a more global one.
|
|
// essentially returns the contribution of a subshape to the inertia of a larger rigid body
|
|
// typical usage:
|
|
// foreach shape s { totalinertia += InertiaContribution(...); }
|
|
// cor - new center of rotation that we are translating to.
|
|
// This could be the center of mass of the compound object.
|
|
// Another application is when an object is attached to something (nail-joint) that is static, in which
|
|
// one easy way to implement this is to lock the center of rotation and adjust the inertia accordingly.
|
|
// position & orientation - is the current pose or transform of the shape.
|
|
// Obviously position, orientation and cor are all described wrt the same reference frame.
|
|
// shape_com and shape_inertia are the center of mass and the inertia of the shape in the local coordinate system of that shape.
|
|
// To clarify, if a shape happened to be located somewhere else then position or orientation would be different, but
|
|
// com and inertia would be the same.
|
|
float3x3 Identity(1.0f,0,0,0,1.0f,0,0,0,1.0f);
|
|
float3x3 R = orientation.getmatrix();
|
|
float3 r = (shape_com*R + position) - cor;
|
|
return Transpose(R)*shape_inertia*R + (Identity*dot(r,r)-outerprod(r,r))*shape_mass;
|
|
}
|
|
|
|
|
|
|
|
Quaternion Diagonalizer(const float3x3 &A)
|
|
{
|
|
// A must be a symmetric matrix.
|
|
// returns quaternion q such that its corresponding matrix Q
|
|
// can be used to Diagonalize A
|
|
// Diagonal matrix D = Q * A * Transpose(Q); and A = QT*D*Q
|
|
// The rows of q are the eigenvectors D's diagonal is the eigenvalues
|
|
// As per 'row' convention if float3x3 Q = q.getmatrix(); then v*Q = q*v*conj(q)
|
|
int maxsteps=24; // certainly wont need that many.
|
|
int i;
|
|
Quaternion q(0,0,0,1);
|
|
for(i=0;i<maxsteps;i++)
|
|
{
|
|
float3x3 Q = q.getmatrix(); // v*Q == q*v*conj(q)
|
|
float3x3 D = Q * A * Transpose(Q); // A = Q^T*D*Q
|
|
float3 offdiag(D[1][2],D[0][2],D[0][1]); // elements not on the diagonal
|
|
float3 om(fabsf(offdiag.x),fabsf(offdiag.y),fabsf(offdiag.z)); // mag of each offdiag elem
|
|
int k = (om.x>om.y&&om.x>om.z)?0: (om.y>om.z)? 1 : 2; // index of largest element of offdiag
|
|
int k1 = (k+1)%3;
|
|
int k2 = (k+2)%3;
|
|
if(offdiag[k]==0.0f) break; // diagonal already
|
|
float thet = (D[k2][k2]-D[k1][k1])/(2.0f*offdiag[k]);
|
|
float sgn = (thet>0.0f)?1.0f:-1.0f;
|
|
thet *= sgn; // make it positive
|
|
float t = sgn /(thet +((thet<1.E6f)?sqrtf(squared(thet)+1.0f):thet)) ; // sign(T)/(|T|+sqrt(T^2+1))
|
|
float c = 1.0f/sqrtf(squared(t)+1.0f); // c= 1/(t^2+1) , t=s/c
|
|
if(c==1.0f) break; // no room for improvement - reached machine precision.
|
|
Quaternion jr(0,0,0,0); // jacobi rotation for this iteration.
|
|
jr[k] = sgn*sqrtf((1.0f-c)/2.0f); // using 1/2 angle identity sin(a/2) = sqrt((1-cos(a))/2)
|
|
jr[k] *= -1.0f; // since our quat-to-matrix convention was for v*M instead of M*v
|
|
jr.w = sqrtf(1.0f - squared(jr[k]));
|
|
if(jr.w==1.0f) break; // reached limits of floating point precision
|
|
q = q*jr;
|
|
q.Normalize();
|
|
}
|
|
return q;
|
|
}
|
|
|
|
float3 Diagonal(const float3x3 &M)
|
|
{
|
|
return float3(M[0][0],M[1][1],M[2][2]);
|
|
}
|