BulletSim initial checkin

1 files changed, 284 insertions, 0 deletions

diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs
new file mode 100644
index 0000000..7d1592f
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/float4x4.cs
@@ -0,0 +1,284 @@
+/* The MIT License
+ * 
+ * Copyright (c) 2010 Intel Corporation.
+ * All rights reserved.
+ *
+ * Based on the convexdecomposition library from 
+ * <http://codesuppository.googlecode.com> by John W. Ratcliff and Stan Melax.
+ * 
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ * 
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ * 
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+ * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Text;
+
+namespace OpenSim.Region.Physics.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 dst = d.x.x;
+                    float[] tmp = new float[12]; // temp array for pairs
+                    float[] src = new float[16]; // array of transpose source matrix
+                    float det; // determinant
+                    // transpose matrix
+                    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;
+                    }
+                    // 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)
+                    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];
+                    // 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)
+                    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];
+                    // calculate determinant
+            det = src[0] * d.x.x + src[1] * d.x.y + src[2] * d.x.z + src[3] * d.x.w;
+                    // calculate matrix inverse
+                    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); // view space height
+            float w = h / aspect; // view space width
+            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)
+        {
+            // 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 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);
+        }
+    }
+}