public unsafe static void AddVertex(this BuSimplex1To4 obj, ref OpenTK.Vector3 pt)
{
fixed(OpenTK.Vector3 *ptPtr = &pt)
{
obj.AddVertex(ref *(BulletSharp.Math.Vector3 *)ptPtr);
}
}
public override void InternalProcessTriangleIndex(ref Vector3 vertex0, ref Vector3 vertex1, ref Vector3 vertex2, int partId, int triangleIndex)
{
Vector3 scale = _meshShape.LocalScaling;
Vector3 v0 = vertex0 * scale;
Vector3 v1 = vertex1 * scale;
Vector3 v2 = vertex2 * scale;
Vector3 centroid = (v0 + v1 + v2) / 3;
Vector3 normal = (v1 - v0).Cross(v2 - v0);
normal.Normalize();
Vector3 rayFrom = centroid;
Vector3 rayTo = centroid - normal * _depth;
using (var cb = new MyCallback(ref rayFrom, ref rayTo, partId, triangleIndex))
{
_meshShape.ProcessAllTrianglesRayRef(cb, ref rayFrom, ref rayTo);
if (cb.HitFraction < 1)
{
rayTo = Vector3.Lerp(cb.From, cb.To, cb.HitFraction);
//rayTo = cb.From;
//Vector3 to = centroid + normal;
//debugDraw.DrawLine(ref centroid, ref to, ref _redColor);
}
}
var triangle = new BuSimplex1To4(v0, v1, v2, rayTo);
_collisionShape.AddChildShape(Matrix.Identity, triangle);
}
public override void InternalProcessTriangleIndex(ref Vector3 vertex0, ref Vector3 vertex1, ref Vector3 vertex2, int partId, int triangleIndex)
{
Vector3 scale = _meshShape.LocalScaling;
Vector3 v0 = vertex0 * scale;
Vector3 v1 = vertex1 * scale;
Vector3 v2 = vertex2 * scale;
Vector3 centroid = (v0 + v1 + v2) / 3;
Vector3 normal = (v1 - v0).Cross(v2 - v0);
normal.Normalize();
Vector3 rayFrom = centroid;
Vector3 rayTo = centroid - normal * _depth;
MyCallback cb = new MyCallback(ref rayFrom, ref rayTo, partId, triangleIndex);
_meshShape.ProcessAllTrianglesRayRef(cb, ref rayFrom, ref rayTo);
if (cb.HitFraction < 1)
{
rayTo = Vector3.Lerp(cb.From, cb.To, cb.HitFraction);
//rayTo = cb.From;
//gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0));
}
BuSimplex1To4 tet = new BuSimplex1To4(v0, v1, v2, rayTo);
_colShape.AddChildShape(Matrix.Identity, tet);
}
public override void InternalProcessTriangleIndex(ref Vector3 vertex0, ref Vector3 vertex1, ref Vector3 vertex2, int partId, int triangleIndex)
{
Vector3 scale = _meshShape.LocalScaling;
Vector3 v0=vertex0*scale;
Vector3 v1=vertex1*scale;
Vector3 v2=vertex2*scale;
Vector3 centroid = (v0+v1+v2)/3;
Vector3 normal = (v1-v0).Cross(v2-v0);
normal.Normalize();
Vector3 rayFrom = centroid;
Vector3 rayTo = centroid-normal*_depth;
MyCallback cb = new MyCallback(ref rayFrom, ref rayTo, partId, triangleIndex);
_meshShape.ProcessAllTrianglesRayRef(cb, ref rayFrom, ref rayTo);
if (cb.HitFraction < 1)
{
rayTo = Vector3.Lerp(cb.From, cb.To, cb.HitFraction);
//rayTo = cb.From;
//gDebugDraw.drawLine(tr(centroid),tr(centroid+normal),btVector3(1,0,0));
}
BuSimplex1To4 tet = new BuSimplex1To4(v0,v1,v2,rayTo);
_colShape.AddChildShape(Matrix.Identity, tet);
}