mirror of
https://github.com/bulletphysics/bullet3
synced 2024-12-15 22:20:12 +00:00
ab8f16961e
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files. make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type. This commit contains no other changes aside from adding and applying clang-format-all.sh
1146 lines
34 KiB
C++
1146 lines
34 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]);
|
|
}
|