From 302d72701da35b6d481a538c4cb953976d7e9044 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Mon, 20 Jun 2011 17:14:59 -0700 Subject: BulletSim initial checkin --- .../Physics/ConvexDecompositionDotNet/CTri.cs | 341 +++++++++++++++++++++ 1 file changed, 341 insertions(+) create mode 100644 OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs (limited to 'OpenSim/Region/Physics/ConvexDecompositionDotNet/CTri.cs') 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 + * 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 vertices = new List { mP1, mP2, mP3, mNear1, mNear2, mNear3 }; + List indexList = new List(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 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; + } + } +} -- cgit v1.1