public BowlingPin(Game game, Model model, Matrix orientation, Vector3 position)
: base(game, model)
{
body = new Body();
collision = new CollisionSkin(body);
// add a capsule for the main corpus
Primitive capsule = new Capsule(Vector3.Zero, Matrix.Identity, 0.1f, 1.3f);
// add a small box at the buttom
Primitive box = new Box(new Vector3(-0.1f,-0.1f,-0.1f), Matrix.Identity, Vector3.One * 0.2f);
// add a sphere in the middle
Primitive sphere = new Sphere(new Vector3(0.0f, 0.0f, 0.3f), 0.3f);
collision.AddPrimitive(capsule, new MaterialProperties(0.1f, 0.5f, 0.5f));
collision.AddPrimitive(box, new MaterialProperties(0.1f, 0.5f, 0.5f));
collision.AddPrimitive(sphere, new MaterialProperties(0.1f, 0.5f, 0.5f));
body.CollisionSkin = this.collision;
Vector3 com = SetMass(0.5f);
body.MoveTo(position, orientation);
collision.ApplyLocalTransform(new Transform(-com, Matrix.Identity));
body.EnableBody();
this.scale = Vector3.One * 10.0f;
}
public CylinderObject(Game game, float radius, float length, Vector3 position, Model model)
: base(game, model)
{
body = new Body();
collision = new CollisionSkin(body);
if (length - 2.0f * radius < 0.0f)
throw new ArgumentException("Radius must be at least half length");
Capsule middle = new Capsule(Vector3.Zero, Matrix.Identity, radius, length - 2.0f * radius);
float sideLength = 2.0f * radius / (float) Math.Sqrt(2.0d);
Vector3 sides = new Vector3(-0.5f * sideLength, -0.5f * sideLength, -radius);
Box supply0 = new Box(sides, Matrix.Identity,
new Vector3(sideLength, sideLength, length));
Box supply1 = new Box(Vector3.Transform(sides,Matrix.CreateRotationZ(MathHelper.PiOver4)),
Matrix.CreateRotationZ(MathHelper.PiOver4), new Vector3(sideLength, sideLength, length));
collision.AddPrimitive(middle, new MaterialProperties(0.8f, 0.8f, 0.7f));
collision.AddPrimitive(supply0, new MaterialProperties(0.8f, 0.8f, 0.7f));
collision.AddPrimitive(supply1, new MaterialProperties(0.8f, 0.8f, 0.7f));
body.CollisionSkin = this.collision;
Vector3 com = SetMass(1.0f);
collision.ApplyLocalTransform(new Transform(-com, Matrix.Identity));
#region Manually set body inertia
float cylinderMass = body.Mass;
float comOffs = (length - 2.0f * radius) * 0.5f; ;
float Ixx = 0.5f * cylinderMass * radius * radius + cylinderMass * comOffs * comOffs;
float Iyy = 0.25f * cylinderMass * radius * radius + (1.0f / 12.0f) * cylinderMass * length * length + cylinderMass * comOffs * comOffs;
float Izz = Iyy;
body.SetBodyInertia(Ixx, Iyy, Izz);
#endregion
body.MoveTo(position, Matrix.CreateRotationX(MathHelper.PiOver2));
body.EnableBody();
this.scale = new Vector3(radius, radius, length * 0.5f);
}
// Sets up the collision skin
void SetupSkin(float length, float radius)
{
if (length - 2.0f * radius < 0.0f) throw new ArgumentException("Radius must be at least half length");
this.length = length;
this.radius = radius;
Capsule middle = new Capsule(Vector3.Zero, Matrix.Identity, radius, length - 2.0f * radius);
float sideLength = 2.0f * radius / (float)Math.Sqrt(2.0d);
Vector3 sides = new Vector3(-0.5f * sideLength, -0.5f * sideLength, -radius);
Box supply0 = new Box(sides, Matrix.Identity, new Vector3(sideLength, sideLength, length));
Box supply1 = new Box(Vector3.Transform(sides, Matrix.CreateRotationZ(MathHelper.PiOver4)),
Matrix.CreateRotationZ(MathHelper.PiOver4), new Vector3(sideLength, sideLength, length));
CollisionSkin.AddPrimitive(middle, new MaterialProperties(0.8f, 0.8f, 0.7f));
CollisionSkin.AddPrimitive(supply0, new MaterialProperties(0.8f, 0.8f, 0.7f));
CollisionSkin.AddPrimitive(supply1, new MaterialProperties(0.8f, 0.8f, 0.7f));
}
public static bool SegmentCapsuleIntersection(out float tS, Segment seg, Capsule capsule)
{
float bestFrac = float.MaxValue;
tS = 0;
// do the main sides
float sideFrac = float.MaxValue;
if (!SegmentInfiniteCylinderIntersection(out sideFrac, seg,
new Segment(capsule.Position, capsule.Orientation.Backward),
capsule.Radius))
return false; // check this
// only keep this if the side intersection point is within the capsule segment ends
Vector3 sidePos = seg.GetPoint(sideFrac);
if (Vector3.Dot(sidePos - capsule.Position, capsule.Orientation.Backward) < 0.0f)
sideFrac = float.MaxValue;
else if (Vector3.Dot(sidePos - capsule.GetEnd(), capsule.Orientation.Backward) > 0.0f)
sideFrac = float.MaxValue;
// do the two ends
float originFrac = float.MaxValue;
SegmentSphereIntersection(out originFrac, seg, new Sphere(capsule.Position, capsule.Radius));
float endFrac = float.MaxValue; // Check this!
SegmentSphereIntersection(out endFrac, seg, new Sphere(capsule.Position, capsule.Radius));
bestFrac = MathHelper.Min(sideFrac, originFrac);
bestFrac = MathHelper.Min(bestFrac, endFrac);
if (bestFrac <= 1.0f)
{
tS = bestFrac;
return true;
}
return false;
}
public override void OnAdd(Scene scene)
{
base.OnAdd(scene);
scene.AddActor(this);
PhysicsSystem world = scene.GetPhysicsEngine();
Vector3 pos = Vector3.Up * 256 + 15*(new Vector3((float)RandomHelper.RandomGen.NextDouble(), (float)RandomHelper.RandomGen.NextDouble(), (float)RandomHelper.RandomGen.NextDouble())*2-Vector3.One);
//pos.X += (scene.MainTerrain as TerrainHeightmap).GetWidth()*0.5f;
//pos.Z += (scene.MainTerrain as TerrainHeightmap).GetDepth() * 0.5f;
Vector3 normal = Vector3.Up;
//scene.MainTerrain.GenerateRandomTransform(RandomHelper.RandomGen, out pos, out normal);
//pos = pos + Vector3.Up * 5;
body = new CharacterBody();
collision = new CollisionSkin(body);
standCapsule = new Capsule(Vector3.Zero, Matrix.CreateRotationX(MathHelper.PiOver2), 1.0f, 1.778f);
crouchCapsule = new Capsule(Vector3.Zero, Matrix.CreateRotationX(MathHelper.PiOver2), 1.0f, 1.0f);
SetupPosture(false);
collision.AddPrimitive(standCapsule, (int)MaterialTable.MaterialID.NormalRough);
body.CollisionSkin = collision;
Vector3 com = PhysicsHelper.SetMass(75.0f, body, collision);
body.MoveTo(pos + com, Matrix.Identity);
collision.ApplyLocalTransform(new JigLibX.Math.Transform(-com, Matrix.Identity));
body.SetBodyInvInertia(0.0f, 0.0f, 0.0f);
body.AllowFreezing = false;
body.EnableBody();
Transformation = new Transform(body);
ResetState();
}
/// <summary>
/// CollDetectCapsuleStaticMeshOverlap
/// </summary>
/// <param name="oldCapsule"></param>
/// <param name="newCapsule"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
private void CollDetectCapsuleStaticMeshOverlap(Capsule oldCapsule, Capsule newCapsule,
TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
Vector3 body0Pos = (info.Skin0.Owner != null) ? info.Skin0.Owner.OldPosition : Vector3.Zero;
Vector3 body1Pos = (info.Skin1.Owner != null) ? info.Skin1.Owner.OldPosition : Vector3.Zero;
float capsuleTolR = collTolerance + newCapsule.Radius;
float capsuleTolR2 = capsuleTolR * capsuleTolR;
Vector3 collNormal = Vector3.Zero;
BoundingBox bb = BoundingBoxHelper.InitialBox;
BoundingBoxHelper.AddCapsule(newCapsule, ref bb);
unsafe
{
#if USE_STACKALLOC
SmallCollPointInfo* collPts = stackalloc SmallCollPointInfo[MaxLocalStackSCPI];
int* potentialTriangles = stackalloc int[MaxLocalStackTris];
{
{
#else
SmallCollPointInfo[] collPtArray = SCPIStackAlloc();
fixed (SmallCollPointInfo* collPts = collPtArray)
{
int[] potTriArray = IntStackAlloc();
fixed (int* potentialTriangles = potTriArray)
{
#endif
int numCollPts = 0;
int numTriangles = mesh.GetTrianglesIntersectingtAABox(potentialTriangles, MaxLocalStackTris, ref bb);
Vector3 capsuleStart = newCapsule.Position;
Vector3 capsuleEnd = newCapsule.GetEnd();
Matrix4 meshInvTransform = mesh.InverseTransformMatrix;
Vector3 meshSpaceCapsuleStart = Vector3.Transform(capsuleStart, meshInvTransform);
Vector3 meshSpaceCapsuleEnd = Vector3.Transform(capsuleEnd, meshInvTransform);
for (int iTriangle = 0; iTriangle < numTriangles; ++iTriangle)
{
IndexedTriangle meshTriangle = mesh.GetTriangle(potentialTriangles[iTriangle]);
// we do the plane test using the capsule in mesh space
float distToStart = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleStart);
float distToEnd = meshTriangle.Plane.DotCoordinate(meshSpaceCapsuleEnd);
// BEN-BUG-FIX: Fixed by replacing 0.0F with -capsuleTolR.
if ((distToStart > capsuleTolR && distToEnd > capsuleTolR)
|| (distToStart < -capsuleTolR && distToEnd < -capsuleTolR))
continue;
// we now transform the triangle into world space (we could keep leave the mesh alone
// but at this point 3 vector transforms is probably not a major slow down)
int i0, i1, i2;
meshTriangle.GetVertexIndices(out i0, out i1, out i2);
Vector3 triVec0;
Vector3 triVec1;
Vector3 triVec2;
mesh.GetVertex(i0, out triVec0);
mesh.GetVertex(i1, out triVec1);
mesh.GetVertex(i2, out triVec2);
// Deano move tri into world space
Matrix4 transformMatrix = mesh.TransformMatrix;
Vector3.Transform(ref triVec0, ref transformMatrix, out triVec0);
Vector3.Transform(ref triVec1, ref transformMatrix, out triVec1);
Vector3.Transform(ref triVec2, ref transformMatrix, out triVec2);
Triangle triangle = new Triangle(ref triVec0, ref triVec1, ref triVec2);
Segment seg = new Segment(capsuleStart, capsuleEnd - capsuleStart);
float tS, tT0, tT1;
float d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, seg, triangle);
if (d2 < capsuleTolR2)
{
Vector3 oldCapsuleStart = oldCapsule.Position;
Vector3 oldCapsuleEnd = oldCapsule.GetEnd();
Segment oldSeg = new Segment(oldCapsuleStart, oldCapsuleEnd - oldCapsuleStart);
d2 = Distance.SegmentTriangleDistanceSq(out tS, out tT0, out tT1, oldSeg, triangle);
// report result from old position
float dist = (float)System.Math.Sqrt(d2);
float depth = oldCapsule.Radius - dist;
Vector3 pt = triangle.GetPoint(tT0, tT1);
Vector3 collisionN = (d2 > JiggleMath.Epsilon) ? JiggleMath.NormalizeSafe(oldSeg.GetPoint(tS) - pt) :
meshTriangle.Plane.Normal;
if (numCollPts < MaxLocalStackSCPI)
{
// BEN-OPTIMISATION: Reused existing collPts.
collPts[numCollPts].R0 = pt - body0Pos;
collPts[numCollPts].R1 = pt - body1Pos;
collPts[numCollPts++].InitialPenetration = depth;
}
collNormal += collisionN;
}
}
if (numCollPts > 0)
{
JiggleMath.NormalizeSafe(ref collNormal);
collisionFunctor.CollisionNotify(ref info, ref collNormal, collPts, numCollPts);
}
#if USE_STACKALLOC
}
}
#else
}
FreeStackAlloc(potTriArray);
}
FreeStackAlloc(collPtArray);
#endif
}
}
/// <summary>
/// CollDetectCapsulseStaticMeshSweep
/// </summary>
/// <param name="oldCapsule"></param>
/// <param name="newCapsule"></param>
/// <param name="mesh"></param>
/// <param name="info"></param>
/// <param name="collTolerance"></param>
/// <param name="collisionFunctor"></param>
private void CollDetectCapsulseStaticMeshSweep(Capsule oldCapsule, Capsule newCapsule,
TriangleMesh mesh, CollDetectInfo info, float collTolerance, CollisionFunctor collisionFunctor)
{
// really use a swept test - or overlap?
Vector3 delta = newCapsule.Position - oldCapsule.Position;
if (delta.LengthSquared < (0.25f * newCapsule.Radius * newCapsule.Radius))
{
CollDetectCapsuleStaticMeshOverlap(oldCapsule, newCapsule, mesh, info, collTolerance, collisionFunctor);
}
else
{
float capsuleLen = oldCapsule.Length;
float capsuleRadius = oldCapsule.Radius;
int nSpheres = 2 + (int)(capsuleLen / (2.0f * oldCapsule.Radius));
for (int iSphere = 0; iSphere < nSpheres; ++iSphere)
{
float offset = ((float)iSphere) * capsuleLen / ((float)nSpheres - 1.0f);
BoundingSphere oldSphere = new BoundingSphere(oldCapsule.Position + oldCapsule.Orientation.Backward() * offset, capsuleRadius);
BoundingSphere newSphere = new BoundingSphere(newCapsule.Position + newCapsule.Orientation.Backward() * offset, capsuleRadius);
CollDetectSphereStaticMesh.CollDetectSphereStaticMeshSweep(oldSphere, newSphere, mesh, info, collTolerance, collisionFunctor);
}
}
}
public CharacterObject(string id, ObjectType objectType, Transform3D transform, Effect effect,
Texture2D texture, Model model, Color color, float alpha,
float radius, float height, float accelerationRate, float decelerationRate)
: base(id, objectType, transform, effect, texture, model, color, alpha)
{
this.Body = new Character(accelerationRate, decelerationRate);
this.Collision = new CollisionSkin(Body);
this.Body.ExternalData = this;
this.Body.CollisionSkin = this.Collision;
Capsule capsule = new Capsule(Vector3.Zero, Matrix.CreateRotationX(MathHelper.PiOver2), radius, height);
this.Collision.AddPrimitive(capsule, (int)MaterialTable.MaterialID.NormalSmooth);
}
/// <summary>
/// AddCapsule
/// </summary>
/// <param name="capsule"></param>
/// <param name="bb"></param>
public static void AddCapsule(Capsule capsule, ref BoundingBox bb)
{
AddSphere(new BoundingSphere(capsule.Position, capsule.Radius), ref bb);
AddSphere(new BoundingSphere(capsule.Position + capsule.Length * capsule.Orientation.Backward(), capsule.Radius), ref bb);
}
/// <summary>
/// AddCapsule
/// </summary>
/// <param name="capsule"></param>
/// <param name="bb"></param>
public static void AddCapsule(Capsule capsule, ref BoundingBox bb)
{
AddSphere(new Microsoft.Xna.Framework.BoundingSphere(capsule.Position, capsule.Radius), ref bb);
AddSphere(new Microsoft.Xna.Framework.BoundingSphere(capsule.Position + capsule.Length * capsule.Orientation.Backward, capsule.Radius), ref bb);
}
static public void AddCapsule(Capsule capsule, ref BoundingBox bb)
{
AddSphere(new Microsoft.Xna.Framework.BoundingSphere(capsule.Position, capsule.Radius), ref bb);
AddSphere(new Microsoft.Xna.Framework.BoundingSphere(capsule.Position + capsule.Length * capsule.Orientation.Backward, capsule.Radius), ref bb);
}