public bool Find()
{
if (Result != IntersectionResult.NotComputed)
{
return(Result != g3.IntersectionResult.NoIntersection);
}
// Compute the offset origin, edges, and normal.
Vector3d diff = ray.Origin - triangle.V0;
Vector3d edge1 = triangle.V1 - triangle.V0;
Vector3d edge2 = triangle.V2 - triangle.V0;
Vector3d normal = edge1.Cross(edge2);
// Solve Q + t*D = b1*E1 + b2*E2 (Q = kDiff, D = ray direction,
// E1 = kEdge1, E2 = kEdge2, N = Cross(E1,E2)) by
// |Dot(D,N)|*b1 = sign(Dot(D,N))*Dot(D,Cross(Q,E2))
// |Dot(D,N)|*b2 = sign(Dot(D,N))*Dot(D,Cross(E1,Q))
// |Dot(D,N)|*t = -sign(Dot(D,N))*Dot(Q,N)
double DdN = ray.Direction.Dot(normal);
double sign;
if (DdN > MathUtil.ZeroTolerance)
{
sign = 1;
}
else if (DdN < -MathUtil.ZeroTolerance)
{
sign = -1;
DdN = -DdN;
}
else
{
// Ray and triangle are parallel, call it a "no intersection"
// even if the ray does intersect.
Result = IntersectionResult.NoIntersection;
return(false);
}
double DdQxE2 = sign * ray.Direction.Dot(diff.Cross(edge2));
if (DdQxE2 >= 0)
{
double DdE1xQ = sign * ray.Direction.Dot(edge1.Cross(diff));
if (DdE1xQ >= 0)
{
if (DdQxE2 + DdE1xQ <= DdN)
{
// Line intersects triangle, check if ray does.
double QdN = -sign *diff.Dot(normal);
if (QdN >= 0)
{
// Ray intersects triangle.
double inv = (1) / DdN;
RayParameter = QdN * inv;
double mTriBary1 = DdQxE2 * inv;
double mTriBary2 = DdE1xQ * inv;
TriangleBaryCoords = new Vector3d(1 - mTriBary1 - mTriBary2, mTriBary1, mTriBary2);
Type = IntersectionType.Point;
Quantity = 1;
Result = IntersectionResult.Intersects;
return(true);
}
// else: t < 0, no intersection
}
// else: b1+b2 > 1, no intersection
}
// else: b2 < 0, no intersection
}
// else: b1 < 0, no intersection
Result = IntersectionResult.NoIntersection;
return(false);
}
// ported from WildMagic5 Wm5PolyhedralMassProperties
//
// Computes mass/volume, center of mass, and 3x3 intertia tensor
// for a closed mesh. You provide an enumerator over triangles and
// a vertex accessor.
//
// If bodyCoords is 'true', the inertia tensor will be relative to
// body coordinates, if 'false' it is in world coordinates.
//
// the inertia tensor is row-major
public static void MassProperties(
IEnumerable <Index3i> triangle_indices,
Func <int, Vector3d> getVertexF,
out double mass, out Vector3d center, out double[,] inertia3x3,
bool bodyCoords = false)
{
const double oneDiv6 = (1.0 / 6.0);
const double oneDiv24 = (1.0 / 24.0);
const double oneDiv60 = (1.0 / 60.0);
const double oneDiv120 = (1.0 / 120.0);
// order: 1, x, y, z, x^2, y^2, z^2, xy, yz, zx
double[] integral = new double[10] {
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
};
foreach (Index3i tri in triangle_indices)
{
// Get vertices of triangle i.
Vector3d v0 = getVertexF(tri.a);
Vector3d v1 = getVertexF(tri.b);
Vector3d v2 = getVertexF(tri.c);
// Get cross product of edges and normal vector.
Vector3d V1mV0 = v1 - v0;
Vector3d V2mV0 = v2 - v0;
Vector3d N = V1mV0.Cross(V2mV0);
// Compute integral terms.
double tmp0, tmp1, tmp2;
double f1x, f2x, f3x, g0x, g1x, g2x;
tmp0 = v0.x + v1.x;
f1x = tmp0 + v2.x;
tmp1 = v0.x * v0.x;
tmp2 = tmp1 + v1.x * tmp0;
f2x = tmp2 + v2.x * f1x;
f3x = v0.x * tmp1 + v1.x * tmp2 + v2.x * f2x;
g0x = f2x + v0.x * (f1x + v0.x);
g1x = f2x + v1.x * (f1x + v1.x);
g2x = f2x + v2.x * (f1x + v2.x);
double f1y, f2y, f3y, g0y, g1y, g2y;
tmp0 = v0.y + v1.y;
f1y = tmp0 + v2.y;
tmp1 = v0.y * v0.y;
tmp2 = tmp1 + v1.y * tmp0;
f2y = tmp2 + v2.y * f1y;
f3y = v0.y * tmp1 + v1.y * tmp2 + v2.y * f2y;
g0y = f2y + v0.y * (f1y + v0.y);
g1y = f2y + v1.y * (f1y + v1.y);
g2y = f2y + v2.y * (f1y + v2.y);
double f1z, f2z, f3z, g0z, g1z, g2z;
tmp0 = v0.z + v1.z;
f1z = tmp0 + v2.z;
tmp1 = v0.z * v0.z;
tmp2 = tmp1 + v1.z * tmp0;
f2z = tmp2 + v2.z * f1z;
f3z = v0.z * tmp1 + v1.z * tmp2 + v2.z * f2z;
g0z = f2z + v0.z * (f1z + v0.z);
g1z = f2z + v1.z * (f1z + v1.z);
g2z = f2z + v2.z * (f1z + v2.z);
// Update integrals.
integral[0] += N.x * f1x;
integral[1] += N.x * f2x;
integral[2] += N.y * f2y;
integral[3] += N.z * f2z;
integral[4] += N.x * f3x;
integral[5] += N.y * f3y;
integral[6] += N.z * f3z;
integral[7] += N.x * (v0.y * g0x + v1.y * g1x + v2.y * g2x);
integral[8] += N.y * (v0.z * g0y + v1.z * g1y + v2.z * g2y);
integral[9] += N.z * (v0.x * g0z + v1.x * g1z + v2.x * g2z);
}
integral[0] *= oneDiv6;
integral[1] *= oneDiv24;
integral[2] *= oneDiv24;
integral[3] *= oneDiv24;
integral[4] *= oneDiv60;
integral[5] *= oneDiv60;
integral[6] *= oneDiv60;
integral[7] *= oneDiv120;
integral[8] *= oneDiv120;
integral[9] *= oneDiv120;
// mass
mass = integral[0];
// center of mass
center = new Vector3d(integral[1], integral[2], integral[3]) / mass;
// inertia3x3 relative to world origin
inertia3x3 = new double[3, 3];
inertia3x3[0, 0] = integral[5] + integral[6];
inertia3x3[0, 1] = -integral[7];
inertia3x3[0, 2] = -integral[9];
inertia3x3[1, 0] = inertia3x3[0, 1];
inertia3x3[1, 1] = integral[4] + integral[6];
inertia3x3[1, 2] = -integral[8];
inertia3x3[2, 0] = inertia3x3[0, 2];
inertia3x3[2, 1] = inertia3x3[1, 2];
inertia3x3[2, 2] = integral[4] + integral[5];
// inertia3x3 relative to center of mass
if (bodyCoords)
{
inertia3x3[0, 0] -= mass * (center.y * center.y +
center.z * center.z);
inertia3x3[0, 1] += mass * center.x * center.y;
inertia3x3[0, 2] += mass * center.z * center.x;
inertia3x3[1, 0] = inertia3x3[0, 1];
inertia3x3[1, 1] -= mass * (center.z * center.z +
center.x * center.x);
inertia3x3[1, 2] += mass * center.y * center.z;
inertia3x3[2, 0] = inertia3x3[0, 2];
inertia3x3[2, 1] = inertia3x3[1, 2];
inertia3x3[2, 2] -= mass * (center.x * center.x +
center.y * center.y);
}
}
