//! classify points that are closer
public void MergePoints(ref Vector4 plane, float margin, ObjectArray <IndexedVector3> points, int point_count)
{
m_point_count = 0;
m_penetration_depth = -1000.0f;
int[] point_indices = new int[MAX_TRI_CLIPPING];
int _k;
for (_k = 0; _k < point_count; _k++)
{
float _dist = -ClipPolygon.DistancePointPlane(ref plane, ref points.GetRawArray()[_k]) + margin;
if (_dist >= 0.0f)
{
if (_dist > m_penetration_depth)
{
m_penetration_depth = _dist;
point_indices[0] = _k;
m_point_count = 1;
}
else if ((_dist + MathUtil.SIMD_EPSILON) >= m_penetration_depth)
{
point_indices[m_point_count] = _k;
m_point_count++;
}
}
}
for (_k = 0; _k < m_point_count; _k++)
{
m_points[_k] = points[point_indices[_k]];
}
}
//! classify points that are closer
public void MergePoints(ref Vector4 plane, float margin, ObjectArray<IndexedVector3> points, int point_count)
{
m_point_count = 0;
m_penetration_depth = -1000.0f;
int[] point_indices = new int[MAX_TRI_CLIPPING];
int _k;
for (_k = 0; _k < point_count; _k++)
{
float _dist = -ClipPolygon.DistancePointPlane(ref plane, ref points.GetRawArray()[_k]) + margin;
if (_dist >= 0.0f)
{
if (_dist > m_penetration_depth)
{
m_penetration_depth = _dist;
point_indices[0] = _k;
m_point_count = 1;
}
else if ((_dist + MathUtil.SIMD_EPSILON) >= m_penetration_depth)
{
point_indices[m_point_count] = _k;
m_point_count++;
}
}
}
for (_k = 0; _k < m_point_count; _k++)
{
m_points[_k] = points[point_indices[_k]];
}
}
public virtual void ProcessTriangle(ObjectArray<Vector3> triangle,int partId, int triangleIndex)
{
Vector3[] rawData = triangle.GetRawArray();
for (int i=0;i<3;i++)
{
float dot;
Vector3.Dot(ref m_supportVecLocal,ref rawData[i],out dot);
if (dot > m_maxDot)
{
m_maxDot = dot;
m_supportVertexLocal = triangle[i];
}
}
}
//! Clips a polygon by a plane
/*!
*\return The count of the clipped counts
*/
public static int PlaneClipPolygon(
ref Vector4 plane,
ObjectArray <IndexedVector3> polygon_points,
int polygon_point_count,
ObjectArray <IndexedVector3> clipped)
{
int clipped_count = 0;
IndexedVector3[] rawPoints = polygon_points.GetRawArray();
//clip first point
float firstdist = DistancePointPlane(ref plane, ref rawPoints[0]);;
if (!(firstdist > MathUtil.SIMD_EPSILON))
{
clipped[clipped_count] = polygon_points[0];
clipped_count++;
}
float olddist = firstdist;
for (int i = 1; i < polygon_point_count; i++)
{
float dist = DistancePointPlane(ref plane, ref rawPoints[i]);
PlaneClipPolygonCollect(
ref rawPoints[i - 1], ref rawPoints[i],
olddist,
dist,
clipped,
ref clipped_count);
olddist = dist;
}
//RETURN TO FIRST point
PlaneClipPolygonCollect(
ref rawPoints[polygon_point_count - 1], ref rawPoints[0],
olddist,
firstdist,
clipped,
ref clipped_count);
return(clipped_count);
}
//! Clips a polygon by a plane
/*!
*\return The count of the clipped counts
*/
public static int PlaneClipPolygon(
ref Vector4 plane,
ObjectArray<IndexedVector3> polygon_points,
int polygon_point_count,
ObjectArray<IndexedVector3> clipped)
{
int clipped_count = 0;
IndexedVector3[] rawPoints = polygon_points.GetRawArray();
//clip first point
float firstdist = DistancePointPlane(ref plane, ref rawPoints[0]); ;
if (!(firstdist > MathUtil.SIMD_EPSILON))
{
clipped[clipped_count] = polygon_points[0];
clipped_count++;
}
float olddist = firstdist;
for (int i = 1; i < polygon_point_count; i++)
{
float dist = DistancePointPlane(ref plane, ref rawPoints[i]);
PlaneClipPolygonCollect(
ref rawPoints[i - 1], ref rawPoints[i],
olddist,
dist,
clipped,
ref clipped_count);
olddist = dist;
}
//RETURN TO FIRST point
PlaneClipPolygonCollect(
ref rawPoints[polygon_point_count - 1], ref rawPoints[0],
olddist,
firstdist,
clipped,
ref clipped_count);
return clipped_count;
}
public void GetIndices(int face_index, out int i0, out int i1, out int i2)
{
if (indicestype == PHY_ScalarType.PHY_SHORT)
{
ObjectArray <ushort> ushortArray = indexbase as ObjectArray <ushort>;
if (ushortArray != null)
{
ushort[] temp = ushortArray.GetRawArray();
int index = face_index * indexstride;
i0 = temp[index];
i1 = temp[index + 1];
i2 = temp[index + 2];
}
else
{
i0 = 0;
i1 = 1;
i2 = 2;
}
}
else
{
ObjectArray <int> intArray = indexbase as ObjectArray <int>;
if (intArray != null)
{
int[] temp = intArray.GetRawArray();
int index = face_index * indexstride;
i0 = temp[index];
i1 = temp[index + 1];
i2 = temp[index + 2];
}
else
{
i0 = 0;
i1 = 1;
i2 = 2;
}
}
}
//! This function calcs the distance from a 3D plane
public static void PlaneClipPolygonCollect(
ref IndexedVector3 point0,
ref IndexedVector3 point1,
float dist0,
float dist1,
ObjectArray<IndexedVector3> clipped,
ref int clipped_count)
{
bool _prevclassif = (dist0 > MathUtil.SIMD_EPSILON);
bool _classif = (dist1 > MathUtil.SIMD_EPSILON);
if (_classif != _prevclassif)
{
float blendfactor = -dist0 / (dist1 - dist0);
VecBlend(ref clipped.GetRawArray()[clipped_count], ref point0, ref point1, blendfactor);
clipped_count++;
}
if (!_classif)
{
clipped[clipped_count] = point1;
clipped_count++;
}
}
public virtual void StoreIslandActivationState(CollisionWorld collisionWorld)
{
int index = 0;
ObjectArray <CollisionObject> list = collisionWorld.GetCollisionObjectArray();
int length = list.Count;
CollisionObject[] rawList = list.GetRawArray();
for (int i = 0; i < length; ++i)
{
CollisionObject collisionObject = rawList[i];
if (!collisionObject.IsStaticOrKinematicObject())
{
collisionObject.SetIslandTag(m_unionFind.Find(index));
collisionObject.SetCompanionId(-1);
}
else
{
collisionObject.SetIslandTag(-1);
collisionObject.SetCompanionId(-2);
}
index++;
}
}
public virtual void UpdateActivationState(CollisionWorld collisionWorld, IDispatcher dispatcher)
{
InitUnionFind(collisionWorld.GetCollisionObjectArray().Count);
// put the index into m_controllers into m_tag
{
int index = 0;
ObjectArray <CollisionObject> list = collisionWorld.GetCollisionObjectArray();
int length = list.Count;
CollisionObject[] rawList = list.GetRawArray();
for (int i = 0; i < length; ++i)
{
CollisionObject collisionObject = rawList[i];
collisionObject.SetIslandTag(index);
collisionObject.SetCompanionId(-1);
collisionObject.SetHitFraction(1f);
index++;
}
}
// do the union find
FindUnions(dispatcher, collisionWorld);
}
public void FindUnions(IDispatcher dispatcher, CollisionWorld collisionWorld)
{
ObjectArray <BroadphasePair> list = collisionWorld.GetPairCache().GetOverlappingPairArray();
int length = list.Count;
if (length > 0)
{
BroadphasePair[] rawList = list.GetRawArray();
for (int i = 0; i < length; ++i)
{
BroadphasePair collisionPair = rawList[i];
CollisionObject colObj0 = collisionPair.m_pProxy0.m_clientObject as CollisionObject;
CollisionObject colObj1 = collisionPair.m_pProxy1.m_clientObject as CollisionObject;
if (((colObj0 != null) && ((colObj0).MergesSimulationIslands())) &&
((colObj1 != null) && ((colObj1).MergesSimulationIslands())))
{
m_unionFind.Unite((colObj0).GetIslandTag(),
(colObj1).GetIslandTag());
}
}
}
}
public void GetVertex(int vertex_index, out IndexedVector3 vertex)
{
ObjectArray <IndexedVector3> svertices = vertexbase as ObjectArray <IndexedVector3>;
vertex = IndexedVector3.Zero;
if (svertices != null)
{
int index = vertex_index * stride;
vertex = svertices[index] * m_scale.X;
}
else
{
ObjectArray <float> fvertices = vertexbase as ObjectArray <float>;
if (svertices != null)
{
float[] temp = fvertices.GetRawArray();
int index = vertex_index * stride;
vertex.X = temp[index] * m_scale.X;
vertex.Y = temp[index + 1] * m_scale.Y;
vertex.Z = temp[index + 2] * m_scale.Z;
}
}
}
//! This function calcs the distance from a 3D plane
public static void PlaneClipPolygonCollect(
ref IndexedVector3 point0,
ref IndexedVector3 point1,
float dist0,
float dist1,
ObjectArray <IndexedVector3> clipped,
ref int clipped_count)
{
bool _prevclassif = (dist0 > MathUtil.SIMD_EPSILON);
bool _classif = (dist1 > MathUtil.SIMD_EPSILON);
if (_classif != _prevclassif)
{
float blendfactor = -dist0 / (dist1 - dist0);
VecBlend(ref clipped.GetRawArray()[clipped_count], ref point0, ref point1, blendfactor);
clipped_count++;
}
if (!_classif)
{
clipped[clipped_count] = point1;
clipped_count++;
}
}
public override void InitializeDemo()
{
SetCameraDistance(50);
int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
int vertStride = 1;
int indexStride = 3;
BulletGlobals.gContactAddedCallback = new CustomMaterialCombinerCallback();
gVertices = new ObjectArray<IndexedVector3>(totalVerts);
gIndices = new ObjectArray<int>(totalTriangles * 3);
SetVertexPositions(waveheight, 0.0f);
gVertices.GetRawArray()[1].Y = 0.1f;
int index=0;
int i, j;
for (i=0;i<NUM_VERTS_X-1;i++)
{
for (j=0;j<NUM_VERTS_Y-1;j++)
{
#if SWAP_WINDING
#if SHIFT_INDICES
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#else
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
#endif //SHIFT_INDICES
#else //SWAP_WINDING
#if SHIFT_INDICES
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i+1;
#if TEST_INCONSISTENT_WINDING
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#else //TEST_INCONSISTENT_WINDING
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
#endif //TEST_INCONSISTENT_WINDING
#else //SHIFT_INDICES
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = j*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = j*NUM_VERTS_X+i;
gIndices[index++] = (j+1)*NUM_VERTS_X+i+1;
gIndices[index++] = (j+1)*NUM_VERTS_X+i;
#endif //SHIFT_INDICES
#endif //SWAP_WINDING
}
}
m_indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
totalVerts, gVertices, vertStride);
bool useQuantizedAabbCompression = true;
IndexedVector3 aabbMin = new IndexedVector3 (-1000,-1000,-1000);
IndexedVector3 aabbMax = new IndexedVector3(1000, 1000, 1000);
trimeshShape = new BvhTriangleMeshShape(m_indexVertexArrays,useQuantizedAabbCompression,ref aabbMin,ref aabbMax,true);
CollisionShape groundShape = trimeshShape;
TriangleInfoMap triangleInfoMap = new TriangleInfoMap();
InternalEdgeUtility.GenerateInternalEdgeInfo(trimeshShape, triangleInfoMap);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
m_dynamicsWorld.SetDebugDrawer(m_debugDraw);
IndexedVector3 gravity = new IndexedVector3(0,-10,0);
m_dynamicsWorld.SetGravity(ref gravity);
float mass = 0.0f;
IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0,-2,0));
ConvexHullShape colShape = new ConvexHullShape(new List<IndexedVector3>(), 0);
for (int k=0;k<DemoMeshes.TaruVtxCount;k++)
{
IndexedVector3 vtx = DemoMeshes.TaruVtx[k];
colShape.AddPoint(ref vtx);
}
//this will enable polyhedral contact clipping, better quality, slightly slower
//colShape.InitializePolyhedralFeatures();
//the polyhedral contact clipping can use either GJK or SAT test to find the separating axis
m_dynamicsWorld.GetDispatchInfo().m_enableSatConvex=false;
{
//for (int i2 = 0; i2 < 1; i2++)
//{
// startTransform._origin = new IndexedVector3(-10.0f + i2 * 3.0f, 2.2f + i2 * 0.1f, -1.3f);
// RigidBody body = LocalCreateRigidBody(10, startTransform, colShape);
// body.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// body.SetLinearVelocity(new IndexedVector3(0, 0, -1));
// //body->setContactProcessingThreshold(0.f);
//}
}
{
BoxShape colShape2 = new BoxShape(new IndexedVector3(1, 1, 1));
//colShape.InitializePolyhedralFeatures();
m_collisionShapes.Add(colShape2);
startTransform._origin = new IndexedVector3(-16.0f + i * 3.0f, 1.0f + i * 0.1f, -1.3f);
RigidBody body = LocalCreateRigidBody(10, startTransform, colShape2);
body.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
body.SetLinearVelocity(new IndexedVector3(0, 0, -1));
}
startTransform = IndexedMatrix.Identity;
staticBody = LocalCreateRigidBody(mass, startTransform,groundShape);
//staticBody->setContactProcessingThreshold(-0.031f);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT);//STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
m_debugDraw.SetDebugMode(DebugDrawModes.DBG_DrawText | DebugDrawModes.DBG_NoHelpText | DebugDrawModes.DBG_DrawWireframe | DebugDrawModes.DBG_DrawContactPoints);
//base.InitializeDemo();
//ClientResetScene();
}
//! Calculates the exact inertia tensor for this shape
public virtual void CalculateLocalInertia(float mass, ref IndexedVector3 inertia)
{
LockChildShapes();
inertia = IndexedVector3.Zero;
int i = GetNumChildShapes();
float shapemass = mass / ((float)i);
while (i-- != 0)
{
IndexedVector3 temp_inertia;
m_childShapes[i].CalculateLocalInertia(shapemass, out temp_inertia);
if (ChildrenHasTransform())
{
inertia = GImpactMassUtil.GimInertiaAddTransformed(ref inertia, ref temp_inertia, ref m_childTransforms.GetRawArray()[i]);
}
else
{
IndexedMatrix identity = IndexedMatrix.Identity;
inertia = GImpactMassUtil.GimInertiaAddTransformed(ref inertia, ref temp_inertia, ref identity);
}
}
}
public override void InitializeDemo()
{
SetCameraDistance(50);
int totalTriangles = 2 * (NUM_VERTS_X - 1) * (NUM_VERTS_Y - 1);
int vertStride = 1;
int indexStride = 3;
BulletGlobals.gContactAddedCallback = new CustomMaterialCombinerCallback();
gVertices = new ObjectArray <IndexedVector3>(totalVerts);
gIndices = new ObjectArray <int>(totalTriangles * 3);
SetVertexPositions(waveheight, 0.0f);
gVertices.GetRawArray()[1].Y = 0.1f;
int index = 0;
int i, j;
for (i = 0; i < NUM_VERTS_X - 1; i++)
{
for (j = 0; j < NUM_VERTS_Y - 1; j++)
{
#if SWAP_WINDING
#if SHIFT_INDICES
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
#else
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
#endif //SHIFT_INDICES
#else //SWAP_WINDING
#if SHIFT_INDICES
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
#if TEST_INCONSISTENT_WINDING
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
#else //TEST_INCONSISTENT_WINDING
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
#endif //TEST_INCONSISTENT_WINDING
#else //SHIFT_INDICES
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = j * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = j * NUM_VERTS_X + i;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i + 1;
gIndices[index++] = (j + 1) * NUM_VERTS_X + i;
#endif //SHIFT_INDICES
#endif //SWAP_WINDING
}
}
m_indexVertexArrays = new TriangleIndexVertexArray(totalTriangles,
gIndices,
indexStride,
totalVerts, gVertices, vertStride);
bool useQuantizedAabbCompression = true;
IndexedVector3 aabbMin = new IndexedVector3(-1000, -1000, -1000);
IndexedVector3 aabbMax = new IndexedVector3(1000, 1000, 1000);
trimeshShape = new BvhTriangleMeshShape(m_indexVertexArrays, useQuantizedAabbCompression, ref aabbMin, ref aabbMax, true);
CollisionShape groundShape = trimeshShape;
TriangleInfoMap triangleInfoMap = new TriangleInfoMap();
InternalEdgeUtility.GenerateInternalEdgeInfo(trimeshShape, triangleInfoMap);
m_collisionConfiguration = new DefaultCollisionConfiguration();
m_dispatcher = new CollisionDispatcher(m_collisionConfiguration);
m_broadphase = new DbvtBroadphase();
m_constraintSolver = new SequentialImpulseConstraintSolver();
m_dynamicsWorld = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_constraintSolver, m_collisionConfiguration);
m_dynamicsWorld.SetDebugDrawer(m_debugDraw);
IndexedVector3 gravity = new IndexedVector3(0, -10, 0);
m_dynamicsWorld.SetGravity(ref gravity);
float mass = 0.0f;
IndexedMatrix startTransform = IndexedMatrix.CreateTranslation(new IndexedVector3(0, -2, 0));
ConvexHullShape colShape = new ConvexHullShape(new List <IndexedVector3>(), 0);
for (int k = 0; k < DemoMeshes.TaruVtxCount; k++)
{
IndexedVector3 vtx = DemoMeshes.TaruVtx[k];
colShape.AddPoint(ref vtx);
}
//this will enable polyhedral contact clipping, better quality, slightly slower
//colShape.InitializePolyhedralFeatures();
//the polyhedral contact clipping can use either GJK or SAT test to find the separating axis
m_dynamicsWorld.GetDispatchInfo().m_enableSatConvex = false;
{
//for (int i2 = 0; i2 < 1; i2++)
//{
// startTransform._origin = new IndexedVector3(-10.0f + i2 * 3.0f, 2.2f + i2 * 0.1f, -1.3f);
// RigidBody body = LocalCreateRigidBody(10, startTransform, colShape);
// body.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
// body.SetLinearVelocity(new IndexedVector3(0, 0, -1));
// //body->setContactProcessingThreshold(0.f);
//}
}
{
BoxShape colShape2 = new BoxShape(new IndexedVector3(1, 1, 1));
//colShape.InitializePolyhedralFeatures();
m_collisionShapes.Add(colShape2);
startTransform._origin = new IndexedVector3(-16.0f + i * 3.0f, 1.0f + i * 0.1f, -1.3f);
RigidBody body = LocalCreateRigidBody(10, startTransform, colShape2);
body.SetActivationState(ActivationState.DISABLE_DEACTIVATION);
body.SetLinearVelocity(new IndexedVector3(0, 0, -1));
}
startTransform = IndexedMatrix.Identity;
staticBody = LocalCreateRigidBody(mass, startTransform, groundShape);
//staticBody->setContactProcessingThreshold(-0.031f);
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_KINEMATIC_OBJECT); //STATIC_OBJECT);
//enable custom material callback
staticBody.SetCollisionFlags(staticBody.GetCollisionFlags() | CollisionFlags.CF_CUSTOM_MATERIAL_CALLBACK);
m_debugDraw.SetDebugMode(DebugDrawModes.DBG_DrawText | DebugDrawModes.DBG_NoHelpText | DebugDrawModes.DBG_DrawWireframe | DebugDrawModes.DBG_DrawContactPoints);
//base.InitializeDemo();
//ClientResetScene();
}
public virtual void ProcessTriangle(ObjectArray<Vector3> triangle, int partId, int triangleIndex)
{
Vector3[] raw = triangle.GetRawArray();
Vector3 v10;
Vector3 v20;
Vector3.Subtract(ref raw[1],ref raw[0],out v10);
Vector3.Subtract(ref raw[2],ref raw[0],out v20);
Vector3 triangleNormal;
Vector3.Cross(ref v10,ref v20,out triangleNormal);
float dist;
Vector3.Dot(ref raw[0],ref triangleNormal,out dist);
float dist_a;
Vector3.Dot(ref triangleNormal,ref m_from,out dist_a);
dist_a -= dist;
float dist_b;
Vector3.Dot(ref triangleNormal,ref m_to,out dist_b);
dist_b -= dist;
if (dist_a * dist_b >= 0f)
{
return; // same sign
}
//@BP Mod - Backface filtering
if (((m_flags & EFlags.kF_FilterBackfaces) != 0) && (dist_a > 0f))
{
// Backface, skip check
return;
}
float proj_length = dist_a - dist_b;
float distance = (dist_a) / (proj_length);
// Now we have the intersection point on the plane, we'll see if it's inside the triangle
// Add an epsilon as a tolerance for the raycast,
// in case the ray hits exacly on the edge of the triangle.
// It must be scaled for the triangle size.
if (distance < m_hitFraction)
{
float edge_tolerance = triangleNormal.LengthSquared();
edge_tolerance *= -0.0001f;
Vector3 point;
point = MathUtil.Interpolate3(ref m_from, ref m_to, distance);
{
Vector3 v0p;
Vector3.Subtract(ref raw[0],ref point,out v0p);
Vector3 v1p;
Vector3.Subtract(ref raw[1],ref point,out v1p);
Vector3 cp0;
Vector3.Cross(ref v0p,ref v1p,out cp0);
float dot;
Vector3.Dot(ref cp0, ref triangleNormal, out dot);
if (dot >= edge_tolerance)
{
Vector3 v2p;
Vector3.Subtract(ref raw[2],ref point,out v2p);
Vector3 cp1; //= Vector3.Cross(v1p,v2p);
Vector3.Cross(ref v1p, ref v2p, out cp1);
float dot2;
Vector3.Dot(ref cp1, ref triangleNormal, out dot2);
if (dot2 >= edge_tolerance)
{
Vector3 cp2;
Vector3.Cross(ref v2p, ref v0p, out cp2);
float dot3;
Vector3.Dot(ref cp2, ref triangleNormal, out dot3);
if (dot3 >= edge_tolerance)
{
//@BP Mod
// Triangle normal isn't normalized
triangleNormal.Normalize();
//@BP Mod - Allow for unflipped normal when raycasting against backfaces
if (((m_flags & EFlags.kF_KeepUnflippedNormal) != 0) || (dist_a <= 0.0f))
{
Vector3 negNormal = -triangleNormal;
m_hitFraction = ReportHit(ref negNormal, distance, partId, triangleIndex);
}
else
{
m_hitFraction = ReportHit(ref triangleNormal, distance, partId, triangleIndex);
}
}
}
}
}
}
}
