BulletSim initial checkin

1 files changed, 341 insertions, 0 deletions

diff --git a/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs b/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs
new file mode 100644
index 0000000..4d84c44
--- /dev/null
+++ b/OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs
@@ -0,0 +1,341 @@
+/* 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;
+
+namespace OpenSim.Region.Physics.ConvexDecompositionDotNet
+{
+    public class Wpoint
+    {
+        public float3 mPoint;
+        public float mWeight;
+
+        public Wpoint(float3 p, float w)
+        {
+            mPoint = p;
+            mWeight = w;
+        }
+    }
+
+    public class CTri
+    {
+        private const int WSCALE = 4;
+
+        public float3 mP1;
+        public float3 mP2;
+        public float3 mP3;
+        public float3 mNear1;
+        public float3 mNear2;
+        public float3 mNear3;
+        public float3 mNormal;
+        public float mPlaneD;
+        public float mConcavity;
+        public float mC1;
+        public float mC2;
+        public float mC3;
+        public int mI1;
+        public int mI2;
+        public int mI3;
+        public int mProcessed; // already been added...
+
+        public CTri(float3 p1, float3 p2, float3 p3, int i1, int i2, int i3)
+        {
+            mProcessed = 0;
+            mI1 = i1;
+            mI2 = i2;
+            mI3 = i3;
+
+            mP1 = new float3(p1);
+            mP2 = new float3(p2);
+            mP3 = new float3(p3);
+
+            mNear1 = new float3();
+            mNear2 = new float3();
+            mNear3 = new float3();
+
+            mNormal = new float3();
+            mPlaneD = mNormal.ComputePlane(mP1, mP2, mP3);
+        }
+
+        public float Facing(CTri t)
+        {
+            return float3.dot(mNormal, t.mNormal);
+        }
+
+        public bool clip(float3 start, ref float3 end)
+        {
+            float3 sect = new float3();
+            bool hit = lineIntersectsTriangle(start, end, mP1, mP2, mP3, ref sect);
+
+            if (hit)
+                end = sect;
+            return hit;
+        }
+
+        public bool Concave(float3 p, ref float distance, ref float3 n)
+        {
+            n.NearestPointInTriangle(p, mP1, mP2, mP3);
+            distance = p.Distance(n);
+            return true;
+        }
+
+        public void addTri(int[] indices, int i1, int i2, int i3, ref int tcount)
+        {
+            indices[tcount * 3 + 0] = i1;
+            indices[tcount * 3 + 1] = i2;
+            indices[tcount * 3 + 2] = i3;
+            tcount++;
+        }
+
+        public float getVolume()
+        {
+            int[] indices = new int[8 * 3];
+
+            int tcount = 0;
+
+            addTri(indices, 0, 1, 2, ref tcount);
+            addTri(indices, 3, 4, 5, ref tcount);
+
+            addTri(indices, 0, 3, 4, ref tcount);
+            addTri(indices, 0, 4, 1, ref tcount);
+
+            addTri(indices, 1, 4, 5, ref tcount);
+            addTri(indices, 1, 5, 2, ref tcount);
+
+            addTri(indices, 0, 3, 5, ref tcount);
+            addTri(indices, 0, 5, 2, ref tcount);
+
+            List<float3> vertices = new List<float3> { mP1, mP2, mP3, mNear1, mNear2, mNear3 };
+            List<int> indexList = new List<int>(indices);
+
+            float v = Concavity.computeMeshVolume(vertices, indexList);
+            return v;
+        }
+
+        public float raySect(float3 p, float3 dir, ref float3 sect)
+        {
+            float4 plane = new float4();
+
+            plane.x = mNormal.x;
+            plane.y = mNormal.y;
+            plane.z = mNormal.z;
+            plane.w = mPlaneD;
+
+            float3 dest = p + dir * 100000f;
+
+            intersect(p, dest, ref sect, plane);
+
+            return sect.Distance(p); // return the intersection distance
+        }
+
+        public float planeDistance(float3 p)
+        {
+            float4 plane = new float4();
+
+            plane.x = mNormal.x;
+            plane.y = mNormal.y;
+            plane.z = mNormal.z;
+            plane.w = mPlaneD;
+
+            return DistToPt(p, plane);
+        }
+
+        public bool samePlane(CTri t)
+        {
+            const float THRESH = 0.001f;
+            float dd = Math.Abs(t.mPlaneD - mPlaneD);
+            if (dd > THRESH)
+                return false;
+            dd = Math.Abs(t.mNormal.x - mNormal.x);
+            if (dd > THRESH)
+                return false;
+            dd = Math.Abs(t.mNormal.y - mNormal.y);
+            if (dd > THRESH)
+                return false;
+            dd = Math.Abs(t.mNormal.z - mNormal.z);
+            if (dd > THRESH)
+                return false;
+            return true;
+        }
+
+        public bool hasIndex(int i)
+        {
+            if (i == mI1 || i == mI2 || i == mI3)
+                return true;
+            return false;
+        }
+
+        public bool sharesEdge(CTri t)
+        {
+            bool ret = false;
+            uint count = 0;
+
+            if (t.hasIndex(mI1))
+                count++;
+            if (t.hasIndex(mI2))
+                count++;
+            if (t.hasIndex(mI3))
+                count++;
+
+            if (count >= 2)
+                ret = true;
+
+            return ret;
+        }
+
+        public float area()
+        {
+            float a = mConcavity * mP1.Area(mP2, mP3);
+            return a;
+        }
+
+        public void addWeighted(List<Wpoint> list)
+        {
+            Wpoint p1 = new Wpoint(mP1, mC1);
+            Wpoint p2 = new Wpoint(mP2, mC2);
+            Wpoint p3 = new Wpoint(mP3, mC3);
+
+            float3 d1 = mNear1 - mP1;
+            float3 d2 = mNear2 - mP2;
+            float3 d3 = mNear3 - mP3;
+
+            d1 *= WSCALE;
+            d2 *= WSCALE;
+            d3 *= WSCALE;
+
+            d1 = d1 + mP1;
+            d2 = d2 + mP2;
+            d3 = d3 + mP3;
+
+            Wpoint p4 = new Wpoint(d1, mC1);
+            Wpoint p5 = new Wpoint(d2, mC2);
+            Wpoint p6 = new Wpoint(d3, mC3);
+
+            list.Add(p1);
+            list.Add(p2);
+            list.Add(p3);
+
+            list.Add(p4);
+            list.Add(p5);
+            list.Add(p6);
+        }
+
+        private static float DistToPt(float3 p, float4 plane)
+            {
+                    float x = p.x;
+                    float y = p.y;
+                    float z = p.z;
+                    float d = x*plane.x + y*plane.y + z*plane.z + plane.w;
+                    return d;
+            }
+
+        private static void intersect(float3 p1, float3 p2, ref float3 split, float4 plane)
+        {
+            float dp1 = DistToPt(p1, plane);
+
+            float3 dir = new float3();
+            dir.x = p2[0] - p1[0];
+            dir.y = p2[1] - p1[1];
+            dir.z = p2[2] - p1[2];
+
+            float dot1 = dir[0] * plane[0] + dir[1] * plane[1] + dir[2] * plane[2];
+            float dot2 = dp1 - plane[3];
+
+            float t = -(plane[3] + dot2) / dot1;
+
+            split.x = (dir[0] * t) + p1[0];
+            split.y = (dir[1] * t) + p1[1];
+            split.z = (dir[2] * t) + p1[2];
+        }
+
+        private static bool rayIntersectsTriangle(float3 p, float3 d, float3 v0, float3 v1, float3 v2, out float t)
+        {
+            t = 0f;
+
+            float3 e1, e2, h, s, q;
+            float a, f, u, v;
+
+            e1 = v1 - v0;
+            e2 = v2 - v0;
+            h = float3.cross(d, e2);
+            a = float3.dot(e1, h);
+
+            if (a > -0.00001f && a < 0.00001f)
+                return false;
+
+            f = 1f / a;
+            s = p - v0;
+            u = f * float3.dot(s, h);
+
+            if (u < 0.0f || u > 1.0f)
+                return false;
+
+            q = float3.cross(s, e1);
+            v = f * float3.dot(d, q);
+            if (v < 0.0f || u + v > 1.0f)
+                return false;
+
+            // at this stage we can compute t to find out where
+            // the intersection point is on the line
+            t = f * float3.dot(e2, q);
+            if (t > 0f) // ray intersection
+                return true;
+            else // this means that there is a line intersection but not a ray intersection
+                return false;
+        }
+
+        private static bool lineIntersectsTriangle(float3 rayStart, float3 rayEnd, float3 p1, float3 p2, float3 p3, ref float3 sect)
+        {
+            float3 dir = rayEnd - rayStart;
+
+            float d = (float)Math.Sqrt(dir[0] * dir[0] + dir[1] * dir[1] + dir[2] * dir[2]);
+            float r = 1.0f / d;
+
+            dir *= r;
+
+            float t;
+            bool ret = rayIntersectsTriangle(rayStart, dir, p1, p2, p3, out t);
+
+            if (ret)
+            {
+                if (t > d)
+                {
+                    sect.x = rayStart.x + dir.x * t;
+                    sect.y = rayStart.y + dir.y * t;
+                    sect.z = rayStart.z + dir.z * t;
+                }
+                else
+                {
+                    ret = false;
+                }
+            }
+
+            return ret;
+        }
+    }
+}