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 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 float4(float4 f)
{
x = f.x;
y = f.y;
z = f.z;
w = f.w;
}
private static PlaneTriResult getSidePlane(float3 p, float4 plane, float epsilon)
{
float d = DistToPt(p, plane);
if ((d + epsilon) > 0f)
return PlaneTriResult.PTR_FRONT; // it is 'in front' within the provided epsilon value.
return PlaneTriResult.PTR_BACK;
}
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);
}
// assumes that the points are on opposite sides of the plane!
private static void intersect(float3 p1, float3 p2, float3 split, float4 plane)
{
float dp1 = DistToPt(p1, plane);
float[] dir = new float[3];
dir[0] = p2[0] - p1[0];
dir[1] = p2[1] - p1[1];
dir[2] = 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];
}
// compute's how 'concave' this object is and returns the total volume of the
// convex hull as well as the volume of the 'concavity' which was found.
public static float computeConcavity(List<float3> vertices, List<int> indices, ref float4 plane, ref float volume)
{
float cret = 0f;
volume = 1f;
HullResult result = new HullResult();
HullDesc desc = new HullDesc();
desc.MaxFaces = 256;
desc.MaxVertices = 256;
desc.SetHullFlag(HullFlag.QF_TRIANGLES);
desc.Vertices = vertices;
HullError ret = HullUtils.CreateConvexHull(desc, ref result);
if (ret == HullError.QE_OK)
{
volume = computeMeshVolume2(result.OutputVertices, result.Indices);
// ok..now..for each triangle on the original mesh..
// we extrude the points to the nearest point on the hull.
List<CTri> tris = new List<CTri>();
for (int i = 0; i < result.Indices.Count / 3; i++)
{
int i1 = result.Indices[i * 3 + 0];
int i2 = result.Indices[i * 3 + 1];
int i3 = result.Indices[i * 3 + 2];
float3 p1 = result.OutputVertices[i1];
float3 p2 = result.OutputVertices[i2];
float3 p3 = result.OutputVertices[i3];
CTri t = new CTri(p1, p2, p3, i1, i2, i3);
tris.Add(t);
}
// we have not pre-computed the plane equation for each triangle in the convex hull..
float totalVolume = 0;
List<CTri> ftris = new List<CTri>(); // 'feature' triangles.
List<CTri> input_mesh = new List<CTri>();
for (int i = 0; i < indices.Count / 3; i++)
{
int i1 = indices[i * 3 + 0];
int i2 = indices[i * 3 + 1];
int i3 = indices[i * 3 + 2];
float3 p1 = vertices[i1];
float3 p2 = vertices[i2];
float3 p3 = vertices[i3];
CTri t = new CTri(p1, p2, p3, i1, i2, i3);
input_mesh.Add(t);
}
for (int i = 0; i < indices.Count / 3; i++)
{
int i1 = indices[i * 3 + 0];
int i2 = indices[i * 3 + 1];
int i3 = indices[i * 3 + 2];
float3 p1 = vertices[i1];
float3 p2 = vertices[i2];
float3 p3 = vertices[i3];
CTri t = new CTri(p1, p2, p3, i1, i2, i3);
featureMatch(t, tris, input_mesh);
if (t.mConcavity > 0.05f)
{
float v = t.getVolume();
totalVolume += v;
ftris.Add(t);
}
}
SplitPlane.computeSplitPlane(vertices, indices, ref plane);
cret = totalVolume;
}
return cret;
}
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;
}
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 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 static bool computeSplitPlane(List<float3> vertices, List<int> indices, ref float4 plane)
{
float[] bmin = { Single.MaxValue, Single.MaxValue, Single.MaxValue };
float[] bmax = { Single.MinValue, Single.MinValue, Single.MinValue };
for (int i = 0; i < vertices.Count; i++)
{
float3 p = vertices[i];
if (p[0] < bmin[0])
bmin[0] = p[0];
if (p[1] < bmin[1])
bmin[1] = p[1];
if (p[2] < bmin[2])
bmin[2] = p[2];
if (p[0] > bmax[0])
bmax[0] = p[0];
if (p[1] > bmax[1])
bmax[1] = p[1];
if (p[2] > bmax[2])
bmax[2] = p[2];
}
float dx = bmax[0] - bmin[0];
float dy = bmax[1] - bmin[1];
float dz = bmax[2] - bmin[2];
float laxis = dx;
int axis = 0;
if (dy > dx)
{
axis = 1;
laxis = dy;
}
if (dz > dx && dz > dy)
{
axis = 2;
laxis = dz;
}
float[] p1 = new float[3];
float[] p2 = new float[3];
float[] p3 = new float[3];
p3[0] = p2[0] = p1[0] = bmin[0] + dx * 0.5f;
p3[1] = p2[1] = p1[1] = bmin[1] + dy * 0.5f;
p3[2] = p2[2] = p1[2] = bmin[2] + dz * 0.5f;
Rect3d b = new Rect3d(bmin, bmax);
Rect3d b1 = new Rect3d();
Rect3d b2 = new Rect3d();
splitRect(axis, b, b1, b2, p1);
switch (axis)
{
case 0:
p2[1] = bmin[1];
p2[2] = bmin[2];
if (dz > dy)
{
p3[1] = bmax[1];
p3[2] = bmin[2];
}
else
{
p3[1] = bmin[1];
p3[2] = bmax[2];
}
break;
case 1:
p2[0] = bmin[0];
p2[2] = bmin[2];
if (dx > dz)
{
p3[0] = bmax[0];
p3[2] = bmin[2];
}
else
{
p3[0] = bmin[0];
p3[2] = bmax[2];
}
break;
case 2:
p2[0] = bmin[0];
p2[1] = bmin[1];
if (dx > dy)
{
p3[0] = bmax[0];
p3[1] = bmin[1];
}
else
{
p3[0] = bmin[0];
p3[1] = bmax[1];
}
break;
}
computePlane(p1, p2, p3, plane);
return true;
}
public static float4 cmul(float4 a, float4 b)
{
return new float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
}
public static float4 cmul(float4 a, float4 b)
{
return(new float4(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w));
}
public static PlaneTriResult planeTriIntersection(float4 plane, FaceTri triangle, float epsilon, ref float3[] front, out int fcount, ref float3[] back, out int bcount)
{
fcount = 0;
bcount = 0;
// get the three vertices of the triangle.
float3 p1 = triangle.P1;
float3 p2 = triangle.P2;
float3 p3 = triangle.P3;
PlaneTriResult r1 = getSidePlane(p1, plane, epsilon); // compute the side of the plane each vertex is on
PlaneTriResult r2 = getSidePlane(p2, plane, epsilon);
PlaneTriResult r3 = getSidePlane(p3, plane, epsilon);
if (r1 == r2 && r1 == r3) // if all three vertices are on the same side of the plane.
{
if (r1 == PlaneTriResult.PTR_FRONT) // if all three are in front of the plane, then copy to the 'front' output triangle.
{
add(p1, front, ref fcount);
add(p2, front, ref fcount);
add(p3, front, ref fcount);
}
else
{
add(p1, back, ref bcount); // if all three are in 'back' then copy to the 'back' output triangle.
add(p2, back, ref bcount);
add(p3, back, ref bcount);
}
return r1; // if all three points are on the same side of the plane return result
}
// ok.. we need to split the triangle at the plane.
// First test ray segment P1 to P2
if (r1 == r2) // if these are both on the same side...
{
if (r1 == PlaneTriResult.PTR_FRONT)
{
add(p1, front, ref fcount);
add(p2, front, ref fcount);
}
else
{
add(p1, back, ref bcount);
add(p2, back, ref bcount);
}
}
else
{
float3 split = new float3();
intersect(p1, p2, split, plane);
if (r1 == PlaneTriResult.PTR_FRONT)
{
add(p1, front, ref fcount);
add(split, front, ref fcount);
add(split, back, ref bcount);
add(p2, back, ref bcount);
}
else
{
add(p1, back, ref bcount);
add(split, back, ref bcount);
add(split, front, ref fcount);
add(p2, front, ref fcount);
}
}
// Next test ray segment P2 to P3
if (r2 == r3) // if these are both on the same side...
{
if (r3 == PlaneTriResult.PTR_FRONT)
{
add(p3, front, ref fcount);
}
else
{
add(p3, back, ref bcount);
}
}
else
{
float3 split = new float3(); // split the point
intersect(p2, p3, split, plane);
if (r3 == PlaneTriResult.PTR_FRONT)
{
add(split, front, ref fcount);
add(split, back, ref bcount);
add(p3, front, ref fcount);
}
else
{
add(split, front, ref fcount);
add(split, back, ref bcount);
add(p3, back, ref bcount);
}
}
// Next test ray segment P3 to P1
if (r3 != r1) // if these are both on the same side...
{
float3 split = new float3(); // split the point
intersect(p3, p1, split, plane);
if (r1 == PlaneTriResult.PTR_FRONT)
{
add(split, front, ref fcount);
add(split, back, ref bcount);
}
else
{
add(split, front, ref fcount);
add(split, back, ref bcount);
}
}
return PlaneTriResult.PTR_SPLIT;
}
private static float DistToPt(float3 p, float4 plane)
{
return p.x * plane.x + p.y * plane.y + p.z * plane.z + plane.w;
}
public static void calcConvexDecomposition(List <float3> vertices, List <int> indices, ConvexDecompositionCallback callback, float masterVolume, int depth,
int maxDepth, float concavePercent, float mergePercent)
{
float4 plane = new float4();
bool split = false;
if (depth < maxDepth)
{
float volume = 0f;
float c = Concavity.computeConcavity(vertices, indices, ref plane, ref volume);
if (depth == 0)
{
masterVolume = volume;
}
float percent = (c * 100.0f) / masterVolume;
if (percent > concavePercent) // if great than 5% of the total volume is concave, go ahead and keep splitting.
{
split = true;
}
}
if (depth >= maxDepth || !split)
{
HullResult result = new HullResult();
HullDesc desc = new HullDesc();
desc.SetHullFlag(HullFlag.QF_TRIANGLES);
desc.Vertices = vertices;
HullError ret = HullUtils.CreateConvexHull(desc, ref result);
if (ret == HullError.QE_OK)
{
ConvexResult r = new ConvexResult(result.OutputVertices, result.Indices);
callback(r);
}
return;
}
List <int> ifront = new List <int>();
List <int> iback = new List <int>();
VertexPool vfront = new VertexPool();
VertexPool vback = new VertexPool();
// ok..now we are going to 'split' all of the input triangles against this plane!
for (int i = 0; i < indices.Count / 3; i++)
{
int i1 = indices[i * 3 + 0];
int i2 = indices[i * 3 + 1];
int i3 = indices[i * 3 + 2];
FaceTri t = new FaceTri(vertices, i1, i2, i3);
float3[] front = new float3[4];
float3[] back = new float3[4];
int fcount = 0;
int bcount = 0;
PlaneTriResult result = PlaneTri.planeTriIntersection(plane, t, 0.00001f, ref front, out fcount, ref back, out bcount);
if (fcount > 4 || bcount > 4)
{
result = PlaneTri.planeTriIntersection(plane, t, 0.00001f, ref front, out fcount, ref back, out bcount);
}
switch (result)
{
case PlaneTriResult.PTR_FRONT:
Debug.Assert(fcount == 3);
addTri(vfront, ifront, front[0], front[1], front[2]);
break;
case PlaneTriResult.PTR_BACK:
Debug.Assert(bcount == 3);
addTri(vback, iback, back[0], back[1], back[2]);
break;
case PlaneTriResult.PTR_SPLIT:
Debug.Assert(fcount >= 3 && fcount <= 4);
Debug.Assert(bcount >= 3 && bcount <= 4);
addTri(vfront, ifront, front[0], front[1], front[2]);
addTri(vback, iback, back[0], back[1], back[2]);
if (fcount == 4)
{
addTri(vfront, ifront, front[0], front[2], front[3]);
}
if (bcount == 4)
{
addTri(vback, iback, back[0], back[2], back[3]);
}
break;
}
}
// ok... here we recursively call
if (ifront.Count > 0)
{
int vcount = vfront.GetSize();
List <float3> vertices2 = vfront.GetVertices();
for (int i = 0; i < vertices2.Count; i++)
{
vertices2[i] = new float3(vertices2[i]);
}
int tcount = ifront.Count / 3;
calcConvexDecomposition(vertices2, ifront, callback, masterVolume, depth + 1, maxDepth, concavePercent, mergePercent);
}
ifront.Clear();
vfront.Clear();
if (iback.Count > 0)
{
int vcount = vback.GetSize();
List <float3> vertices2 = vback.GetVertices();
int tcount = iback.Count / 3;
calcConvexDecomposition(vertices2, iback, callback, masterVolume, depth + 1, maxDepth, concavePercent, mergePercent);
}
iback.Clear();
vback.Clear();
}
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();
}
}
internal static void computePlane(float[] A, float[] B, float[] C, float4 plane)
{
float vx = (B[0] - C[0]);
float vy = (B[1] - C[1]);
float vz = (B[2] - C[2]);
float wx = (A[0] - B[0]);
float wy = (A[1] - B[1]);
float wz = (A[2] - B[2]);
float vw_x = vy * wz - vz * wy;
float vw_y = vz * wx - vx * wz;
float vw_z = vx * wy - vy * wx;
float mag = (float)Math.Sqrt((vw_x * vw_x) + (vw_y * vw_y) + (vw_z * vw_z));
if (mag < 0.000001f)
{
mag = 0;
}
else
{
mag = 1.0f / mag;
}
float x = vw_x * mag;
float y = vw_y * mag;
float z = vw_z * mag;
float D = 0.0f - ((x * A[0]) + (y * A[1]) + (z * A[2]));
plane.x = x;
plane.y = y;
plane.z = z;
plane.w = D;
}
