/// <summary>
/// Builds a planar collision skin for the entity.
/// </summary>
/// <param name="normal">Normal of the plane.</param>
/// <param name="distance">
/// Distance along the normal to the plane. For instance, if the normal is (0,1,0), that
/// is, the Y axis, the created plane will be y = distance.
/// </param>
public void BuildCollisionPlane(Vector3 normal, float distance)
{
JigLibX.Geometry.Plane plane = new JigLibX.Geometry.Plane(normal, distance);
mCollisionSkin.RemoveAllPrimitives();
mCollisionSkin.AddPrimitive(plane, mMaterialProperties);
UpdateMaterialPropsOnCollisionSkin();
UpdateMass();
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Sphere oldSphere = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Sphere;
Sphere newSphere = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Sphere;
JigLibX.Geometry.Plane oldPlane = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as JigLibX.Geometry.Plane;
JigLibX.Geometry.Plane newPlane = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as JigLibX.Geometry.Plane;
Matrix newPlaneInvTransform = newPlane.InverseTransformMatrix;
Matrix oldPlaneInvTransform = oldPlane.InverseTransformMatrix;
Vector3 oldSpherePos = Vector3.Transform(oldSphere.Position, oldPlaneInvTransform);
Vector3 newSpherePos = Vector3.Transform(newSphere.Position, newPlaneInvTransform);
// consider it a contact if either old or new are touching
float oldDist = Distance.PointPlaneDistance(oldSpherePos, oldPlane);
float newDist = Distance.PointPlaneDistance(newSpherePos, newPlane);
if (System.Math.Min(newDist, oldDist) > collTolerance + newSphere.Radius)
{
return;
}
// collision - record depth using the old values
float oldDepth = oldSphere.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldSphere.Position - oldSphere.Radius * oldPlane.Normal;
#if WINDOWS_PHONE
this.collInfo[0] = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth);
collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, this.collInfo, 1);
#else
unsafe
{
SmallCollPointInfo collInfo = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth);
collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, &collInfo, 1);
}
#endif
}
public static bool RayPlaneIntersection(out float t, Ray ray, Plane plane)
{
float dot = Vector3.Dot(ray.Dir, plane.Normal);
if (System.Math.Abs(dot) < JiggleMath.Epsilon)
{
t = 0.0f;
return false;
}
float dist = Distance.PointPlaneDistance(ray.Origin, plane);
t = -dist / dot;
return (t >= 0.0f);
}
public static bool LinePlaneIntersection(out float t, Line line, Plane plane)
{
float dot = Vector3.Dot(line.Dir, plane.Normal);
if (System.Math.Abs(dot) < JiggleMath.Epsilon)
{
t = 0.0f;
return false;
}
float dist = Distance.PointPlaneDistance(line.Origin,plane);
t = -dist / dot;
return true;
}
public Plane GetInverse()
{
Plane plane = new Plane(this.normal, this.d);
plane.Invert();
return plane;
}
public override Primitive Clone()
{
Plane newPlane = new Plane(this.Normal, this.D);
newPlane.Transform = Transform;
return newPlane;
}
/// <summary>
/// PointPlaneDistance
/// </summary>
/// <param name="pt"></param>
/// <param name="plane"></param>
/// <returns>float</returns>
public static float PointPlaneDistance(ref Vector3 pt, Plane plane)
{
float num0;
Vector3.Dot(ref plane.normal, ref pt, out num0);
return plane.D + num0;
}
/// <summary>
/// PointPlaneDistance
/// </summary>
/// <param name="pt"></param>
/// <param name="plane"></param>
/// <returns>float</returns>
public static float PointPlaneDistance(Vector3 pt, Plane plane)
{
return Vector3.Dot(plane.Normal, pt) + plane.D;
}
/// <summary>
/// Detect BoxPlane Collisions.
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
Box oldBox = info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0) as Box;
Box newBox = info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0) as Box;
JPlane oldPlane = info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1) as JPlane;
JPlane newPlane = info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1) as JPlane;
Matrix newPlaneInvTransform = newPlane.InverseTransformMatrix;
Vector3 newBoxCen = Vector3.Transform(newBox.GetCentre(), newPlaneInvTransform);
// quick check
float centreDist = Distance.PointPlaneDistance(newBoxCen, newPlane);
if (centreDist > collTolerance + newBox.GetBoundingRadiusAroundCentre())
{
return;
}
Matrix oldPlaneInvTransform = oldPlane.InverseTransformMatrix;
Vector3[] newPts;
newBox.GetCornerPoints(out newPts);
Vector3[] oldPts;
oldBox.GetCornerPoints(out oldPts);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
for (int i = 0; i < 8; ++i)
{
Vector3.Transform(ref oldPts[i], ref oldPlaneInvTransform, out oldTransPts[i]);
Vector3.Transform(ref newPts[i], ref newPlaneInvTransform, out newPts[i]);
float oldDepth = -Distance.PointPlaneDistance(ref oldTransPts[i], oldPlane);
float newDepth = -Distance.PointPlaneDistance(ref newPts[i], newPlane);
if (MathHelper.Max(oldDepth, newDepth) > -collTolerance)
{
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Now reuses instead of reallocating.
collPts[numCollPts].R0 = oldPts[i] - body0Pos;
collPts[numCollPts].R1 = oldPts[i] - body1Pos;
collPts[numCollPts++].InitialPenetration = oldDepth;
}
}
}
if (numCollPts > 0)
{
collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, collPts, numCollPts);
}
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
/// <summary>
///
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = (Capsule)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
Capsule newCapsule = (Capsule)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);
JigLibX.Geometry.Plane oldPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
JigLibX.Geometry.Plane newPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);
Matrix newPlaneInvTransform = newPlane.InverseTransformMatrix;
Matrix oldPlaneInvTransform = oldPlane.InverseTransformMatrix;
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo *collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed(SmallCollPointInfo *collPts = collPtArray)
#endif
{
int numCollPts = 0;
// the start
{
Vector3 oldCapsuleStartPos = Vector3.Transform(oldCapsule.Position, oldPlaneInvTransform);
Vector3 newCapsuleStartPos = Vector3.Transform(newCapsule.Position, newPlaneInvTransform);
float oldDist = Distance.PointPlaneDistance(oldCapsuleStartPos, oldPlane);
float newDist = Distance.PointPlaneDistance(newCapsuleStartPos, newPlane);
if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position8(s)
Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * oldPlane.Normal;
collPts[numCollPts++] = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth);
}
}
// the end
{
Vector3 oldCapsuleEndPos = Vector3.Transform(oldCapsule.GetEnd(), oldPlaneInvTransform);
Vector3 newCapsuleEndPos = Vector3.Transform(newCapsule.GetEnd(), newPlaneInvTransform);
float oldDist = Distance.PointPlaneDistance(oldCapsuleEndPos, oldPlane);
float newDist = Distance.PointPlaneDistance(newCapsuleEndPos, newPlane);
if (System.Math.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldCapsule.GetEnd() - oldCapsule.Radius * oldPlane.Normal;
collPts[numCollPts++] = new SmallCollPointInfo(worldPos - body0Pos, worldPos - body1Pos, oldDepth);
}
if (numCollPts > 0)
{
collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, collPts, numCollPts);
}
}
}
#if !USE_STACKALLOC
FreeStackAlloc(collPtArray);
#endif
}
}
public void GetHeightAndNormal(out float h, out Vector3 normal,Vector3 point)
{
float x = point.X;
float z = point.Z;
x = MathHelper.Clamp(x, xMin, xMax);
z = MathHelper.Clamp(z, zMin, zMax);
int i0 = (int)((x - xMin) / dx);
int j0 = (int)((point.Z - zMin) / dz);
i0 = (int)MathHelper.Clamp((int)i0, 0, mHeights.Nx - 1);
j0 = (int)MathHelper.Clamp((int)j0, 0, mHeights.Nz - 1);
int i1 = i0 + 1;
int j1 = j0 + 1;
if (i1 >= (int)mHeights.Nx) i1 = mHeights.Nx - 1;
if (j1 >= (int)mHeights.Nz) j1 = mHeights.Nz - 1;
float iFrac = (x - (i0 * dx + xMin)) / dx;
float jFrac = (z - (j0 * dz + zMin)) / dz;
iFrac = MathHelper.Clamp(iFrac, 0.0f, 1.0f);
jFrac = MathHelper.Clamp(jFrac, 0.0f, 1.0f);
float h00 = mHeights[i0, j0];
float h01 = mHeights[i0, j1];
float h10 = mHeights[i1, j0];
float h11 = mHeights[i1, j1];
// All the triangles are orientated the same way.
// work out the normal, then z is in the plane of this normal
if ((i0 == i1) && (j0 == j1))
{
normal = Vector3.Up;
}
else if (i0 == i1)
{
Vector3 right = Vector3.Right;
normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz),right);
normal.Normalize();
}
if (j0 == j1)
{
Vector3 backw = Vector3.Backward;
normal = Vector3.Cross(backw, new Vector3(dx, h10 - h00, 0.0f));
normal.Normalize();
}
else if (iFrac > jFrac)
{
normal = Vector3.Cross(new Vector3(dx, h11 - h00, dz), new Vector3(dx, h10 - h00, 0.0f));
normal.Normalize();
}
else
{
normal = Vector3.Cross(new Vector3(0.0f, h01 - h00, dz), new Vector3(dx, h11 - h00, dz));
normal.Normalize();
}
// get the plane equation
// h00 is in all the triangles
JiggleMath.NormalizeSafe(ref normal);
Plane plane = new Plane(normal, new Vector3((i0 * dx + xMin), h00, (j0 * dz + zMin)));
h = Distance.PointPlaneDistance(point,plane);
}
/// <summary>
/// CollDetect
/// </summary>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
public override void CollDetect(CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
if (info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0).Type == this.Type1)
{
CollisionSkin skinSwap = info.Skin0;
info.Skin0 = info.Skin1;
info.Skin1 = skinSwap;
int primSwap = info.IndexPrim0;
info.IndexPrim0 = info.IndexPrim1;
info.IndexPrim1 = primSwap;
}
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
// todo - proper swept test
Capsule oldCapsule = (Capsule)info.Skin0.GetPrimitiveOldWorld(info.IndexPrim0);
Capsule newCapsule = (Capsule)info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0);
JigLibX.Geometry.Plane oldPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveOldWorld(info.IndexPrim1);
JigLibX.Geometry.Plane newPlane = (JigLibX.Geometry.Plane)info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1);
Matrix newPlaneInvTransform = newPlane.InverseTransformMatrix;
Matrix oldPlaneInvTransform = oldPlane.InverseTransformMatrix;
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
{
int numCollPts = 0;
// the start
{
Vector3 oldCapsuleStartPos = Vector3.Transform(oldCapsule.Position, oldPlaneInvTransform);
Vector3 newCapsuleStartPos = Vector3.Transform(newCapsule.Position, newPlaneInvTransform);
float oldDist = Distance.PointPlaneDistance(oldCapsuleStartPos, oldPlane);
float newDist = Distance.PointPlaneDistance(newCapsuleStartPos, newPlane);
if (MathHelper.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position8(s)
Vector3 worldPos = oldCapsule.Position - oldCapsule.Radius * oldPlane.Normal;
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPtArray[numCollPts].R0 = worldPos - body0Pos;
collPtArray[numCollPts].R1 = worldPos - body1Pos;
collPtArray[numCollPts++].InitialPenetration = oldDepth;
}
}
// the end
{
Vector3 oldCapsuleEndPos = Vector3.Transform(oldCapsule.GetEnd(), oldPlaneInvTransform);
Vector3 newCapsuleEndPos = Vector3.Transform(newCapsule.GetEnd(), newPlaneInvTransform);
float oldDist = Distance.PointPlaneDistance(oldCapsuleEndPos, oldPlane);
float newDist = Distance.PointPlaneDistance(newCapsuleEndPos, newPlane);
if (System.Math.Min(newDist, oldDist) < collTolerance + newCapsule.Radius)
{
float oldDepth = oldCapsule.Radius - oldDist;
// calc the world position based on the old position(s)
Vector3 worldPos = oldCapsule.GetEnd() - oldCapsule.Radius * oldPlane.Normal;
// BEN-OPTIMISATION: Now reuses existing collPts instead of reallocating.
collPtArray[numCollPts].R0 = worldPos - body0Pos;
collPtArray[numCollPts].R1 = worldPos - body1Pos;
collPtArray[numCollPts++].InitialPenetration = oldDepth;
}
if (numCollPts > 0)
{
collisionFunctor.CollisionNotify(ref info, ref oldPlane.normal, collPtArray, numCollPts);
}
}
FreeStackAlloc(collPtArray);
}
}
public static bool SegmentPlaneIntersection(out float tS, Segment seg, Plane plane)
{
float denom = Vector3.Dot(plane.Normal, seg.Delta);
if (System.Math.Abs(denom) > JiggleMath.Epsilon)
{
float t = -(Vector3.Dot(plane.Normal, seg.Origin) + plane.D) / denom;
if (t < 0.0f || t > 1.0f)
{
tS = 0.0f;
return false;
}
tS = t;
return true;
}
else
{
// parallel - return false even if it's in the plane
tS = 0.0f;
return false;
}
}
public void CreatePlane(Vector3 normal, float d)
{
this.collisionSkin = new CollisionSkin(null);
JigLibX.Geometry.Plane primitivePlane =
new JigLibX.Geometry.Plane(normal, d);
MaterialProperties materialProp = new MaterialProperties(
elasticity, staticRoughness, dynamicRoughness);
CollisionSkin.AddPrimitive(primitivePlane, materialProp);
PhysicsSystem.CurrentPhysicsSystem.CollisionSystem.AddCollisionSkin(this.CollisionSkin);
Body.CollisionSkin = CollisionSkin;
CollisionSkin.Owner = Body;
}
