123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284 |
-
- using System;
- using System.Collections.Generic;
- using System.Linq;
- using System.Text;
- namespace OpenSim.Region.PhysicsModules.ConvexDecompositionDotNet
- {
- public class float4x4
- {
- public float4 x = new float4();
- public float4 y = new float4();
- public float4 z = new float4();
- public float4 w = new float4();
- public float4x4()
- {
- }
- public float4x4(float4 _x, float4 _y, float4 _z, float4 _w)
- {
- x = new float4(_x);
- y = new float4(_y);
- z = new float4(_z);
- w = new float4(_w);
- }
- public float4x4(
- float m00, float m01, float m02, float m03,
- float m10, float m11, float m12, float m13,
- float m20, float m21, float m22, float m23,
- float m30, float m31, float m32, float m33)
- {
- x = new float4(m00, m01, m02, m03);
- y = new float4(m10, m11, m12, m13);
- z = new float4(m20, m21, m22, m23);
- w = new float4(m30, m31, m32, m33);
- }
- public float4x4(float4x4 m)
- {
- x = new float4(m.x);
- y = new float4(m.y);
- z = new float4(m.z);
- w = new float4(m.w);
- }
- public float4 this[int i]
- {
- get
- {
- switch (i)
- {
- case 0: return x;
- case 1: return y;
- case 2: return z;
- case 3: return w;
- }
- throw new ArgumentOutOfRangeException();
- }
- set
- {
- switch (i)
- {
- case 0: x = value; return;
- case 1: y = value; return;
- case 2: z = value; return;
- case 3: w = value; return;
- }
- throw new ArgumentOutOfRangeException();
- }
- }
- public override int GetHashCode()
- {
- return x.GetHashCode() ^ y.GetHashCode() ^ z.GetHashCode() ^ w.GetHashCode();
- }
- public override bool Equals(object obj)
- {
- float4x4 m = obj as float4x4;
- if (m == null)
- return false;
- return this == m;
- }
- public static float4x4 operator *(float4x4 a, float4x4 b)
- {
- return new float4x4(a.x * b, a.y * b, a.z * b, a.w * b);
- }
- public static bool operator ==(float4x4 a, float4x4 b)
- {
- return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w);
- }
- public static bool operator !=(float4x4 a, float4x4 b)
- {
- return !(a == b);
- }
- public static float4x4 Inverse(float4x4 m)
- {
- float4x4 d = new float4x4();
-
- float[] tmp = new float[12];
- float[] src = new float[16];
- float det;
-
- for (int i = 0; i < 4; i++)
- {
- src[i] = m[i].x;
- src[i + 4] = m[i].y;
- src[i + 8] = m[i].z;
- src[i + 12] = m[i].w;
- }
-
- 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];
-
- d.x.x = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
- d.x.x -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
- d.x.y = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
- d.x.y -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
- d.x.z = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
- d.x.z -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
- d.x.w = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
- d.x.w -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
- d.y.x = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
- d.y.x -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
- d.y.y = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
- d.y.y -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
- d.y.z = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
- d.y.z -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
- d.y.w = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
- d.y.w -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
-
- 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];
-
- d.z.x = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
- d.z.x -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
- d.z.y = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
- d.z.y -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
- d.z.z = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
- d.z.z -= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
- d.z.w = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
- d.z.w-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
- d.w.x = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
- d.w.x-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
- d.w.y = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
- d.w.y-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
- d.w.z = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
- d.w.z-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
- d.w.w = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
- d.w.w-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
-
- det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w;
-
- det = 1/det;
- for (int j = 0; j < 4; j++)
- d[j] *= det;
- return d;
- }
- public static float4x4 MatrixRigidInverse(float4x4 m)
- {
- float4x4 trans_inverse = MatrixTranslation(-m.w.xyz());
- float4x4 rot = new float4x4(m);
- rot.w = new float4(0f, 0f, 0f, 1f);
- return trans_inverse * MatrixTranspose(rot);
- }
- public static float4x4 MatrixTranspose(float4x4 m)
- {
- return new 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);
- }
- public static float4x4 MatrixPerspectiveFov(float fovy, float aspect, float zn, float zf)
- {
- float h = 1.0f / (float)Math.Tan(fovy / 2.0f);
- float w = h / aspect;
- return new float4x4(w, 0, 0, 0, 0, h, 0, 0, 0, 0, zf / (zn - zf), -1, 0, 0, zn * zf / (zn - zf), 0);
- }
- public static float4x4 MatrixTranslation(float3 t)
- {
- return new float4x4(1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1, 0, t.x, t.y, t.z, 1);
- }
- public static float4x4 MatrixRotationZ(float angle_radians)
- {
- float s = (float)Math.Sin(angle_radians);
- float c = (float)Math.Cos(angle_radians);
- return new float4x4(c, s, 0, 0, -s, c, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
- }
- public static float4x4 MatrixLookAt(float3 eye, float3 at, float3 up)
- {
- float4x4 m = new float4x4();
- m.w.w = 1.0f;
- m.w.setxyz(eye);
- m.z.setxyz(float3.normalize(eye - at));
- m.x.setxyz(float3.normalize(float3.cross(up, m.z.xyz())));
- m.y.setxyz(float3.cross(m.z.xyz(), m.x.xyz()));
- return MatrixRigidInverse(m);
- }
- public static float4x4 MatrixFromQuatVec(Quaternion q, float3 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 new 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);
- }
- }
- }
|