opensim-development – Rev 1
?pathlinks?
/* 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;
}
}
}