public CollisionSkin()
{
this.ID = idCounter++;
this.owner = null;
collSystem = null;
}
public CollisionSkin(Body owner)
{
this.ID = idCounter++;
this.owner = owner;
collSystem = null;
}
public void Initialise(Body body, Vector3 pointOnBody, Vector3 worldPosition)
{
this.body = body;
this.pointOnBody = pointOnBody;
this.worldPosition = worldPosition;
if (body!=null) body.AddConstraint(this);
}
/// <summary>
/// Constructor.
/// </summary>
/// <param name="body"></param>
/// <param name="pointOnBody">pointOnBody is in body coords</param>
/// <param name="worldPosition"></param>
public ConstraintWorldPoint(Body body, Vector3 pointOnBody, Vector3 worldPosition)
{
Initialise(body, pointOnBody, worldPosition);
}
/// <summary>
/// Finds recursivly all bodies which are touching each other and
/// adds them to a list.
/// </summary>
/// <param name="body">The body with which to start</param>
/// <param name="island">The island contains the bodies which are in contact with
/// each other</param>
private void FindConnected(Body body, CollisionIsland island)
{
if (body.foundIsland || body.Immovable) return;
body.foundIsland = true;
island.Add(body);
foreach (CollisionInfo collision in body.CollisionSkin.Collisions)
{
if (collision.SkinInfo.Skin1.Owner != null)
{
if (collision.SkinInfo.Skin1.Owner == body)
FindConnected(collision.SkinInfo.Skin0.Owner, island);
else
FindConnected(collision.SkinInfo.Skin1.Owner, island);
}
}
}
public override void Destroy()
{
// this is moved here from ConstraintWorldPoint destructor..
if (body != null) body.RemoveConstraint(this);
body = null;
DisableConstraint();
}
/// <summary>
/// Detects all collisions between the body and all the registered
/// collision skins (which should have already had their
/// positions/bounding volumes etc updated). For each potential
/// pair of skins then the predicate (if it exists) will be called
/// to see whether or not to continue. If the skins are closer
/// than collTolerance (+ve value means report objects that aren't
/// quite colliding) then the functor will get called.
/// You can't just loop over all your bodies calling this, because
/// that will double-detect collisions. Use DetectAllCollisions for
/// that.
/// </summary>
public abstract void DetectCollisions(Body body, CollisionFunctor collisionFunctor,
CollisionSkinPredicate2 collisionPredicate, float collTolerance);
public LinearSpringJoint(Body body0, Vector3 attachPoint0, Body body1, Vector3 attachPoint1, float springConstant, float dampingConstant, float breakPoint)
: base(springConstant, dampingConstant, breakPoint)
{
Initialize(body0, attachPoint0, body1, attachPoint1);
}
public void Dispose()
{
_primitive = null;
_body = null;
_skin = null;
}
public void Initialise(Body body0, Vector3 body0Pos, Body body1, Vector3 body1Pos, float maxDistance)
{
this.body0Pos = body0Pos;
this.body1Pos = body1Pos;
this.body0 = body0;
this.body1 = body1;
this.mMaxDistance = maxDistance;
if (body0 != null) this.body0.AddConstraint(this);
if (body1 != null) this.body1.AddConstraint(this);
}
public void Initialise(Body body0, Body body1, Vector3 hingeAxis, Vector3 hingePosRel0,
float hingeHalfWidth, float hingeFwdAngle, float hingeBckAngle, float sidewaysSlack, float damping)
{
this.body0 = body0;
this.body1 = body1;
this.hingeAxis = hingeAxis;
this.hingePosRel0 = hingePosRel0;
this.usingLimit = false;
this.damping = damping;
// tScalar allowedDistance = 0.005f;
this.hingeAxis.Normalize();
Vector3 hingePosRel1 = body0.Position + hingePosRel0 - body1.Position;
// generate the two positions relative to each body
Vector3 relPos0a = hingePosRel0 + hingeHalfWidth * hingeAxis;
Vector3 relPos0b = hingePosRel0 - hingeHalfWidth * hingeAxis;
Vector3 relPos1a = hingePosRel1 + hingeHalfWidth * hingeAxis;
Vector3 relPos1b = hingePosRel1 - hingeHalfWidth * hingeAxis;
float timescale = 1.0f / 20.0f;
float allowedDistanceMid = 0.005f;
float allowedDistanceSide = sidewaysSlack * hingeHalfWidth;
mSidePointConstraints = new ConstraintMaxDistance[2];
mSidePointConstraints[0] = new ConstraintMaxDistance();
mSidePointConstraints[1] = new ConstraintMaxDistance();
mSidePointConstraints[0].Initialise(body0, relPos0a, body1, relPos1a, allowedDistanceSide);
mSidePointConstraints[1].Initialise(body0, relPos0b, body1, relPos1b, allowedDistanceSide);
mMidPointConstraint = new ConstraintPoint();
mMidPointConstraint.Initialise(body0, hingePosRel0, body1, hingePosRel1, allowedDistanceMid, timescale);
if (hingeFwdAngle <= 150) // MAX_HINGE_ANGLE_LIMIT
{
// choose a direction that is perpendicular to the hinge
Vector3 perpDir = Vector3Helper.Up;
if (Vector3.Dot(perpDir, hingeAxis) > 0.1f)
perpDir = Vector3Helper.Right;
// now make it perpendicular to the hinge
Vector3 sideAxis = Vector3.Cross(hingeAxis, perpDir);
perpDir = Vector3.Cross(sideAxis, hingeAxis);
perpDir.Normalize();
// the length of the "arm" TODO take this as a parameter? what's
// the effect of changing it?
float len = 10.0f * hingeHalfWidth;
// Choose a position using that dir. this will be the anchor point
// for body 0. relative to hinge
Vector3 hingeRelAnchorPos0 = perpDir * len;
// anchor point for body 2 is chosen to be in the middle of the
// angle range. relative to hinge
float angleToMiddle = 0.5f * (hingeFwdAngle - hingeBckAngle);
Vector3 hingeRelAnchorPos1 = Vector3.TransformCoordinate(hingeRelAnchorPos0, Matrix.RotationAxis(hingeAxis,MathHelper.ToRadians(-angleToMiddle)));
// work out the "string" length
float hingeHalfAngle = 0.5f * (hingeFwdAngle + hingeBckAngle);
float allowedDistance = len * 2.0f * (float)System.Math.Sin(MathHelper.ToRadians(hingeHalfAngle * 0.5f));
Vector3 hingePos = body1.Position + hingePosRel0;
Vector3 relPos0c = hingePos + hingeRelAnchorPos0 - body0.Position;
Vector3 relPos1c = hingePos + hingeRelAnchorPos1 - body1.Position;
mMaxDistanceConstraint = new ConstraintMaxDistance();
mMaxDistanceConstraint.Initialise(body0, relPos0c, body1, relPos1c, allowedDistance);
usingLimit = true;
}
if (damping <= 0.0f)
damping = -1.0f; // just make sure that a value of 0.0 doesn't mess up...
else
damping = MathHelper.Clamp(damping, 0, 1);
}
public override void DetectCollisions(Body body, CollisionFunctor collisionFunctor, CollisionSkinPredicate2 collisionPredicate, float collTolerance)
{
if (!body.IsActive)
return;
CollDetectInfo info = new CollDetectInfo();
info.Skin0 = body.CollisionSkin; //?!
if (info.Skin0 == null) return;
int bodyPrimitves = info.Skin0.NumPrimitives;
int numSkins = skins.Count;
for (int skin = 0; skin < numSkins; ++skin)
{
info.Skin1 = skins[skin];
if ((info.Skin0 != info.Skin1) && CheckCollidables(info.Skin0, info.Skin1))
{
int primitives = info.Skin1.NumPrimitives;
for (info.IndexPrim0 = 0; info.IndexPrim0 < bodyPrimitves; ++info.IndexPrim0)
{
for (info.IndexPrim1 = 0; info.IndexPrim1 < primitives; ++info.IndexPrim1)
{
DetectFunctor f = GetCollDetectFunctor(info.Skin0.GetPrimitiveNewWorld(info.IndexPrim0).Type,
info.Skin1.GetPrimitiveNewWorld(info.IndexPrim1).Type);
if (f != null) f.CollDetect(info, collTolerance, collisionFunctor);
}
}
}
}
}
private int LessBodyZ(Body body0, Body body1)
{
return (body1.Position.Z > body0.Position.Z) ? -1 : 1;
}
private int LessBodyY(Body body0, Body body1)
{
return (body1.Position.Y > body0.Position.Y) ? -1 : 1;
}
private int LessBodyX(Body body0, Body body1)
{
return (body1.Position.X > body0.Position.X) ? -1 : 1;
}
public BasicController(Body body)
{
_body = body;
}
private void Initialize(Primitive primitive, MaterialProperties materialProperties, PrimitiveProperties primitiveProperties, bool enableBody)
{
float mass;
Vector3 centerOfMass;
Matrix inertiaTensor;
Matrix inertiaTensorCoM;
// Set variables ...
_primitive = primitive;
_primitiveProperties = primitiveProperties;
_materialProperties = materialProperties;
// Create and link Body and CollisionSkin.
_body = new Body();
_skin = new CollisionSkin(_body);
_body.CollisionSkin = _skin;
// Add primitive to CollisionSkin.
_skin.AddPrimitive(primitive, materialProperties);
// Set body properties.
_skin.GetMassProperties(primitiveProperties, out mass, out centerOfMass, out inertiaTensor, out inertiaTensorCoM);
_body.BodyInertia = inertiaTensorCoM;
_body.Mass = mass;
// Sync CollisionSkin and Body.
_body.MoveTo(Vector3.Zero, Matrix.Identity);
_skin.ApplyLocalTransform(new Transform(-centerOfMass, Matrix.Identity));
// Enable Body.
if (enableBody)
{
_body.EnableBody();
}
else
{
_body.DisableBody();
}
}
/// <summary>
/// adds the other body to the list of bodies to be activated if
/// this body moves more than a certain distance from either a
/// previously stored position, or the position passed in.
/// </summary>
/// <param name="pos"></param>
/// <param name="otherBody"></param>
public void AddMovementActivation(Vector3 pos, Body otherBody)
{
if (bodiesToBeActivatedOnMovement.Contains(otherBody)) return;
if (bodiesToBeActivatedOnMovement.Count == 0)
storedPositionForActivation = pos;
bodiesToBeActivatedOnMovement.Add(otherBody);
}
/// <summary>
/// Loads a physics world object.
/// </summary>
/// <param name="filePath"></param>
/// <param name="skin"></param>
/// <param name="body"></param>
/// <param name="massProperties"></param>
/// <returns></returns>
public static bool Load(string filePath, out CollisionSkin skin, out Body body, out PrimitiveProperties primitiveProperties, out MassProperties massProperties)
{
skin = null;
body = null;
primitiveProperties = new PrimitiveProperties(PrimitiveProperties.MassDistributionEnum.Solid, PrimitiveProperties.MassTypeEnum.Mass, 0.001f);
massProperties = MassProperties.Zero;
if (File.Exists(filePath))
{
XmlSerializer xmlSerializer = new XmlSerializer(typeof(PhysicsObjectData));
TextReader textReader = new StreamReader(filePath);
PhysicsObjectData data = (PhysicsObjectData)xmlSerializer.Deserialize(textReader);
textReader.Close();
if (data != null && data.MaterialPrimitivePairs != null && data.MaterialPrimitivePairs.Count > 0)
{
body = new JigLibSDX.Physics.Body();
skin = new JigLibSDX.Collision.CollisionSkin(body);
body.CollisionSkin = skin;
primitiveProperties = data.PrimitiveProperties;
for (int i = 0; i < data.MaterialPrimitivePairs.Count; i++)
{
if (data.MaterialPrimitivePairs[i].MaterialID == (int)MaterialTable.MaterialID.UserDefined)
{
skin.AddPrimitive(data.MaterialPrimitivePairs[i].Primitive, data.MaterialPrimitivePairs[i].MaterialID);
}
else
{
skin.AddPrimitive(data.MaterialPrimitivePairs[i].Primitive, data.MaterialPrimitivePairs[i].MaterialProperties);
}
}
massProperties = data.MassProperties;
body.BodyInertia = massProperties.InertiaTensorCoM;
body.Mass = massProperties.Mass;
body.MoveTo(Vector3.Zero, Matrix.Identity);
skin.ApplyLocalTransform(new Transform(-massProperties.CenterOfMass, Matrix.Identity));
body.EnableBody();
}
return true;
}
else
{
MessageBox.Show("File \"" + filePath + "\" not found.", "Error", MessageBoxButtons.OK, MessageBoxIcon.Error);
return false;
}
}
public ConstraintMaxDistance(Body body0, Vector3 body0Pos, Body body1, Vector3 body1Pos, float maxDistance)
{
Initialise(body0, body0Pos, body1, body1Pos, maxDistance);
}
public void Initialise(Body body0, Vector3 body0Pos, Body body1, Vector3 body1Pos, float allowedDistance, float timescale)
{
this.body0Pos = body0Pos;
this.body1Pos = body1Pos;
this.body0 = body0;
this.body1 = body1;
this.allowedDistance = allowedDistance;
this.timescale = timescale;
if (timescale < JiggleMath.Epsilon)
timescale = JiggleMath.Epsilon;
if (body0 != null) body0.AddConstraint(this);
if (body1 != null) body1.AddConstraint(this);
}
private void Initialize(Body body0, Vector3 attachPoint0, Body body1, Vector3 attachPoint1)
{
_body0 = body0;
_body1 = body1;
_attachPoint0 = attachPoint0;
_attachPoint1 = attachPoint1;
_difference = body1.GetWorldPosition(attachPoint1) - body0.GetWorldPosition(attachPoint0);
_restLength = _difference.Length();
}
/// <summary>
/// Adds the body to the Physic- and the CollisionSystem
/// </summary>
/// <param name="body">Body which should be added to the simulation.</param>
public void AddBody(Body body)
{
if (!bodies.Contains(body))
bodies.Add(body);
else
System.Diagnostics.Debug.WriteLine("Warning: tried to add body to physics but it's already registered");
if ((collisionSystem != null) && (body.CollisionSkin != null))
collisionSystem.AddCollisionSkin(body.CollisionSkin);
}
public override void Destroy()
{
body0 = null;
body1 = null;
DisableConstraint();
}
/// <summary>
/// Removes the body from the Physic- and the CollisionSystem
/// </summary>
/// <param name="body">Body which should be removed form the simulation</param>
/// <returns>True if the Body was successfully removed.</returns>
public bool RemoveBody(Body body)
{
if ((collisionSystem != null) && (body.CollisionSkin != null))
collisionSystem.RemoveCollisionSkin(body.CollisionSkin);
if (!bodies.Contains(body))
return false;
bodies.Remove(body);
return true;
}
public ConstraintPoint(Body body0, Vector3 body0Pos, Body body1, Vector3 body1Pos, float allowedDistance, float timescale)
{
Initialise(body0, body0Pos, body1, body1Pos, allowedDistance, timescale);
}
public BodyPair(Body bodyA, Body bodyB)
{
// if (bodyA > bodyB) {mBodyA = bodyA; mBodyB = bodyB; mRA = rA;}
// else {mBodyA = bodyB; mBodyB = bodyA; mRA = rB;}
int skinId = -1;
if (bodyB != null) skinId = bodyB.ID;
if (bodyA.ID > skinId)
{
this.BodyA = bodyA; this.BodyB = bodyB;
}
else
{
this.BodyA = bodyB; this.BodyB = bodyA;
}
this.RA = Vector3.Zero;
}