// // // Typical 3d vector math code. // By S Melax 1998-2008 // // // #include "vecmath.h" #include // for memcpy #include 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]); }