public cpDampedSpring(cpBody a, cpBody b, cpVect anchr1, cpVect anchr2, float restLength, float stiffness, float damping)
: base(a, b)
{
this.anchorA = anchr1;
this.anchorB = anchr2;
this.restLength = restLength;
this.stiffness = stiffness;
this.damping = damping;
this.springForceFunc = defaultSpringForce;
this.target_vrn = this.v_coef = 0;
this.r1 = this.r2 = null;
this.nMass = 0;
this.n = null;
this.jAcc = 0f;
}
public override void ApplyImpulse(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
cpVect n = this.n;
// compute relative velocity
float vrn = cp.normal_relative_velocity(a, b, this.r1, this.r2, n);
float jnMax = this.maxForce * dt;
// compute normal impulse
float jn = (this.bias - vrn) * this.nMass;
float jnOld = this.jnAcc;
this.jnAcc = cp.cpfclamp(jnOld + jn, -jnMax, jnMax);
jn = this.jnAcc - jnOld;
// apply impulse
cp.apply_impulses(a, b, this.r1, this.r2, cpVect.cpvmult(n, jn));
}
public override void PreStep(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
float moment = a.i_inv + b.i_inv;
cp.AssertSoft(moment != 0.0f, "Unsolvable spring.");
this.iSum = 1.0f / moment;
this.w_coef = 1.0f - cp.cpfexp(-this.damping * dt * moment);
this.target_wrn = 0.0f;
// apply spring torque
float j_spring = this.springTorqueFunc(this, a.a - b.a) * dt;
this.jAcc = j_spring;
a.w -= j_spring * a.i_inv;
b.w += j_spring * b.i_inv;
}
public override void PreStep(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
this.r1 = cpTransform.Vect(a.transform, cpVect.cpvsub(this.anchorA, a.cog));
this.r2 = cpTransform.Vect(b.transform, cpVect.cpvsub(this.anchorB, b.cog));
cpVect delta = cpVect.cpvsub(cpVect.cpvadd(b.p, this.r2), cpVect.cpvadd(a.p, this.r1));
float dist = cpVect.cpvlength(delta);
this.n = cpVect.cpvmult(delta, 1.0f / (dist > 0 ? dist : cp.Infinity));
// calculate mass normal
this.nMass = 1.0f / cp.k_scalar(a, b, this.r1, this.r2, this.n);
// calculate bias velocity
float maxBias = this.maxBias;
this.bias = cp.cpfclamp(-cp.bias_coef(this.errorBias, dt) * (dist - this.dist) / dt, -maxBias, maxBias);
}
public override void ApplyImpulse(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
cpVect r1 = this.r1;
cpVect r2 = this.r2;
// compute relative velocity
cpVect vr = cp.relative_velocity(a, b, r1, r2);
// compute normal impulse
cpVect j = cpMat2x2.Transform(this.k, cpVect.cpvsub(this.bias, vr));
cpVect jOld = this.jAcc;
this.jAcc = cpVect.cpvclamp(cpVect.cpvadd(this.jAcc, j), this.maxForce * dt);
j = cpVect.cpvsub(this.jAcc, jOld);
// apply impulse
cp.apply_impulses(a, b, this.r1, this.r2, j);
}
public bool ShapeQuery(cpShape shape, cpSpaceShapeQueryFunc func, object data)
{
cpBody body = shape.body;
cpBB bb = (body != null ? shape.Update(body.transform) : shape.bb);
ShapeQueryContext context = new ShapeQueryContext(func, data, false);
object ctx = (object)context;
Lock();
{
this.staticShapes.Query(shape, bb,
(o1, o2, s, o3) => ShapeQueryFunc(o1 as cpShape, o2 as cpShape, s, (ShapeQueryContext)o3)
, ctx);
this.dynamicShapes.Query(shape, bb,
(o1, o2, s, o3) => ShapeQueryFunc(o1 as cpShape, o2 as cpShape, s, (ShapeQueryContext)o3)
, ctx);
} Unlock(true);
return(((ShapeQueryContext)ctx).anyCollision);
}
public override void ApplyImpulse(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
// compute relative rotational velocity
float wr = b.w * this.ratio - a.w;
float jMax = this.maxForce * dt;
// compute normal impulse
float j = (this.bias - wr) * this.iSum;
float jOld = this.jAcc;
this.jAcc = cp.cpfclamp(jOld + j, -jMax, jMax);
j = this.jAcc - jOld;
// apply impulse
a.w -= j * a.i_inv * this.ratio_inv;
b.w += j * b.i_inv;
}
public override void ApplyImpulse(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
cpVect n = this.n;
cpVect r1 = this.r1;
cpVect r2 = this.r2;
// compute relative velocity
float vrn = cp.normal_relative_velocity(a, b, r1, r2, n);
// compute velocity loss from drag
float v_damp = (this.target_vrn - vrn) * this.v_coef;
this.target_vrn = vrn + v_damp;
float j_damp = v_damp * this.nMass;
this.jAcc += j_damp;
cp.apply_impulses(a, b, this.r1, this.r2, cpVect.cpvmult(this.n, j_damp));
}
// k1 and k2 are modified by the function to contain the outputs.
public static cpMat2x2 k_tensor(cpBody a, cpBody b, cpVect r1, cpVect r2)
{
float m_sum = a.m_inv + b.m_inv;
// start with Identity*m_sum
float k11 = m_sum, k12 = 0.0f;
float k21 = 0.0f, k22 = m_sum;
// add the influence from r1
float a_i_inv = a.i_inv;
float r1xsq = r1.x * r1.x * a_i_inv;
float r1ysq = r1.y * r1.y * a_i_inv;
float r1nxy = -r1.x * r1.y * a_i_inv;
k11 += r1ysq; k12 += r1nxy;
k21 += r1nxy; k22 += r1xsq;
// add the influnce from r2
float b_i_inv = b.i_inv;
float r2xsq = r2.x * r2.x * b_i_inv;
float r2ysq = r2.y * r2.y * b_i_inv;
float r2nxy = -r2.x * r2.y * b_i_inv;
k11 += r2ysq; k12 += r2nxy;
k21 += r2nxy; k22 += r2xsq;
// invert
float det = k11 * k22 - k12 * k21;
cp.AssertSoft(det != 0.0f, "Unsolvable constraint.");
float det_inv = 1.0f / det;
return(new cpMat2x2(
k22 * det_inv, -k12 * det_inv,
-k21 * det_inv, k11 * det_inv
));
}
/// Add a constraint to the simulation.
public cpConstraint AddConstraint(cpConstraint constraint)
{
cp.AssertHard(constraint.space != this, "You have already added this constraint to this space. You must not add it a second time.");
cp.AssertHard(constraint.space == null, "You have already added this constraint to another space. You cannot add it to a second.");
cp.AssertSpaceUnlocked(this);
cpBody a = constraint.a, b = constraint.b;
cp.AssertHard(a != null && b != null, "Constraint is attached to a NULL body.");
a.Activate();
b.Activate();
this.constraints.Add(constraint);
// Push onto the heads of the bodies' constraint lists
constraint.next_a = a.constraintList; a.constraintList = constraint;
constraint.next_b = b.constraintList; b.constraintList = constraint;
constraint.space = this;
return(constraint);
}
public override void PreStep(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
float angle = this.angle;
float phase = this.phase;
float ratchet = this.ratchet;
float delta = b.a - a.a;
float diff = angle - delta;
float pdist = 0.0f;
if (diff * ratchet > 0.0f)
{
pdist = diff;
}
else
{
this.angle = cp.cpffloor((delta - phase) / ratchet) * ratchet + phase;
}
// calculate moment of inertia coefficient.
this.iSum = 1.0f / (a.i_inv + b.i_inv);
// calculate bias velocity
float maxBias = this.maxBias;
this.bias = cp.cpfclamp(-cp.bias_coef(this.errorBias, dt) * pdist / dt, -maxBias, maxBias);
// If the bias is 0, the joint is not at a limit. Reset the impulse.
if (this.bias == 0)
{
this.jAcc = 0.0f;
}
}
public override void PreStep(float dt)
{
cpBody a = this.a;
cpBody b = this.b;
this.r1 = cpTransform.Vect(a.transform, cpVect.cpvsub(this.anchorA, a.cog));
this.r2 = cpTransform.Vect(b.transform, cpVect.cpvsub(this.anchorB, b.cog));
cpVect delta = cpVect.cpvsub(cpVect.cpvadd(b.p, this.r2), cpVect.cpvadd(a.p, this.r1));
float dist = cpVect.cpvlength(delta);
float pdist = 0.0f;
if (dist > this.max)
{
pdist = dist - this.max;
this.n = cpVect.cpvnormalize(delta);
}
else if (dist < this.min)
{
pdist = this.min - dist;
this.n = cpVect.cpvneg(cpVect.cpvnormalize(delta));
}
else
{
this.n = cpVect.Zero;
this.jnAcc = 0.0f;
}
// calculate mass normal
this.nMass = 1.0f / cp.k_scalar(a, b, this.r1, this.r2, this.n);
// calculate bias velocity
float maxBias = this.maxBias;
this.bias = cp.cpfclamp(-cp.bias_coef(this.errorBias, dt) * pdist / dt, -maxBias, maxBias);
}
public static float k_scalar_body(cpBody body, cpVect r, cpVect n)
{
var rcn = cpVect.cpvcross(r, n);
return(body.m_inv + body.i_inv * rcn * rcn);
}
public static void apply_bias_impulses(cpBody a, cpBody b, cpVect r1, cpVect r2, cpVect j)
{
apply_bias_impulse(a, cpVect.cpvneg(j), r1);
apply_bias_impulse(b, j, r2);
}
public static void apply_bias_impulse(cpBody body, cpVect j, cpVect r)
{
body.v_bias = cpVect.cpvadd(body.v_bias, cpVect.cpvmult(j, body.m_inv));
body.w_bias += body.i_inv * cpVect.cpvcross(r, j);
}
public static float normal_relative_velocity(cpBody a, cpBody b, cpVect r1, cpVect r2, cpVect n)
{
return(cpVect.cpvdot(relative_velocity(a, b, r1, r2), n));
}
public cpPivotJoint(cpBody a, cpBody b, cpVect pivot)
: this(a, b,
(a != null ? a.WorldToLocal(pivot) : pivot),
(b != null ? b.WorldToLocal(pivot) : pivot))
{
}
public cpPolyShape(cpBody body, int count, cpVect[] verts, float radius)
: base(body, new cpShapeMassInfo())
{
InitRaw(count, verts, radius);
}
public cpComponentNode(cpBody root, cpBody next, float idleTime)
{
this.root = root;
this.next = next;
this.idleTime = idleTime;
}
public virtual cpConstraint Next(cpBody body)
{
return(this.a == body ? this.next_a : this.next_b);
}
public void ProcessComponents(float dt)
{
var sleep = (this.sleepTimeThreshold != cp.Infinity);
var bodies = this.dynamicBodies;
// These checks can be removed at some stage (if DEBUG == undefined)
for (var i = 0; i < bodies.Count; i++)
{
var body = bodies[i];
cp.AssertSoft(body.nodeNext == null, "Internal Error: Dangling next pointer detected in contact graph.");
cp.AssertSoft(body.nodeRoot == null, "Internal Error: Dangling root pointer detected in contact graph.");
}
// Calculate the kinetic energy of all the bodies
if (sleep)
{
var dv = this.idleSpeedThreshold;
var dvsq = (dv != 0 ? dv * dv : cpVect.cpvlengthsq(this.gravity) * dt * dt);
for (var i = 0; i < bodies.Count; i++)
{
// TODO should make a separate array for kinematic bodies.
if (bodies[i].bodyType != cpBodyType.DYNAMIC)
{
continue;
}
// Need to deal with infinite mass objects
var keThreshold = (dvsq > 0 ? bodies[i].m * dvsq : 0.0f);
bodies[i].nodeIdleTime = (bodies[i].KineticEnergy() > keThreshold ? 0 : bodies[i].nodeIdleTime + dt);
}
}
// Awaken any sleeping bodies found and then push arbiters to the bodies' lists.
List <cpArbiter> arbiters = this.arbiters; // new List<cpArbiter>();
var count = arbiters.Count; //FIX: we cannot read the count values of the array because it changes inside
for (int i = 0; i < count; i++)
{
cpArbiter arb = arbiters[i];
cpBody a = arb.body_a, b = arb.body_b;
if (sleep)
{
if (b.bodyType == cpBodyType.KINEMATIC || a.IsSleeping())
{
a.Activate();
}
if (a.bodyType == cpBodyType.KINEMATIC || b.IsSleeping())
{
b.Activate();
}
}
a.PushArbiter(arb);
b.PushArbiter(arb);
}
if (sleep)
{
// Bodies should be held active if connected by a joint to a non-static rouge body.
var constraints = this.constraints;
for (var i = 0; i < constraints.Count; i++)
{
cpConstraint constraint = constraints[i];
cpBody a = constraint.a, b = constraint.b;
if (b.bodyType == cpBodyType.KINEMATIC)
{
a.Activate();
}
if (a.bodyType == cpBodyType.KINEMATIC)
{
b.Activate();
}
}
// Generate components and deactivate sleeping ones
for (var i = 0; i < bodies.Count;)
{
var body = bodies[i];
if (cp.ComponentRoot(body) == null)
{
// Body not in a component yet. Perform a DFS to flood fill mark
// the component in the contact graph using this body as the root.
FloodFillComponent(body, body);
// Check if the component should be put to sleep.
if (!ComponentActive(body, this.sleepTimeThreshold))
{
this.sleepingComponents.Add(body);
//CP_BODY_FOREACH_COMPONENT
for (var other = body; other != null; other = other.nodeNext)
{
this.DeactivateBody(other);
}
// deactivateBody() removed the current body from the list.
// Skip incrementing the index counter.
continue;
}
}
i++;
// Only sleeping bodies retain their component node pointers.
body.nodeRoot = null;
body.nodeNext = null;
}
}
}
public static float SetAngle(cpBody body, float angle)
{
body.a = angle;
body.AssertSaneBody();
return(angle);
}
/// Allocate and initialize a cpBody.
public static cpBody New(float mass, float moment)
{
cpBody tmp = new cpBody(mass, moment);
return(tmp);
}
public cpArbiter Next(cpBody body)
{
return(this.body_a == body ? this.thread_a.next : this.thread_b.next);
}
public static cpArbiterThread ThreadForBody(cpArbiter arb, cpBody body)
{
return(arb.body_a == body ? arb.thread_a : arb.thread_b);
}
public void Update(cpCollisionInfo info, cpSpace space)
{
cpShape a = info.a, b = info.b;
// For collisions between two similar primitive types, the order could have been swapped since the last frame.
this.a = a; this.body_a = a.body;
this.b = b; this.body_b = b.body;
// Iterate over the possible pairs to look for hash value matches.
for (int i = 0; i < info.count; i++)
{
cpContact con = info.arr[i];
// r1 and r2 store absolute offsets at init time.
// Need to convert them to relative offsets.
con.r1 = cpVect.cpvsub(con.r1, a.body.p);
con.r2 = cpVect.cpvsub(con.r2, b.body.p);
// Cached impulses are not zeroed at init time.
con.jnAcc = con.jtAcc = 0.0f;
for (int j = 0; j < this.Count; j++)
{
cpContact old = this.contacts[j];
// This could trigger false positives, but is fairly unlikely nor serious if it does.
if (con.hash == old.hash)
{
// Copy the persistant contact information.
con.jnAcc = old.jnAcc;
con.jtAcc = old.jtAcc;
}
}
}
//TODO: revise
this.contacts = info.arr;
//this.count = info.count;
this.n = info.n;
this.e = a.e * b.e;
this.u = a.u * b.u;
cpVect surface_vr = cpVect.cpvsub(b.surfaceV, a.surfaceV);
this.surface_vr = cpVect.cpvsub(surface_vr, cpVect.cpvmult(info.n, cpVect.cpvdot(surface_vr, info.n)));
ulong typeA = info.a.type, typeB = info.b.type;
cpCollisionHandler defaultHandler = space.defaultHandler;
cpCollisionHandler handler = this.handler = space.LookupHandler(typeA, typeB, defaultHandler);
// Check if the types match, but don't swap for a default handler which use the wildcard for type A.
bool swapped = this.swapped = (typeA != handler.typeA && handler.typeA != cp.WILDCARD_COLLISION_TYPE);
if (handler != defaultHandler || space.usesWildcards)
{
// The order of the main handler swaps the wildcard handlers too. Uffda.
this.handlerA = space.LookupHandler(swapped ? typeB : typeA, cp.WILDCARD_COLLISION_TYPE, cpCollisionHandler.cpCollisionHandlerDoNothing);
this.handlerB = space.LookupHandler(swapped ? typeA : typeB, cp.WILDCARD_COLLISION_TYPE, cpCollisionHandler.cpCollisionHandlerDoNothing);
}
// mark it as new if it's been cached
if (this.state == cpArbiterState.Cached)
{
this.state = cpArbiterState.FirstCollision;
}
}
public static cpBody ComponentRoot(cpBody body)
{
return(body != null ? body.nodeRoot : null);
}
public void SetBody(cpBody body)
{
cp.AssertHard(!Active(), "You cannot change the body on an active shape. You must remove the shape from the space before changing the body.");
this.body = body;
}
// Used for disposing of collision handlers.
//static void FreeWrap(void* ptr, void* unused) { cpfree(ptr); }
//MARK: Memory Management Functions
public cpSpace()
{
#if DEBUG
Console.WriteLine("Initializing cpSpace - Chipmunk v{0} (Debug Enabled)\n", cp.cpVersionString);
Console.WriteLine("Compile with -DNDEBUG defined to disable debug mode and runtime assertion checks\n");
#endif
/// Number of iterations to use in the impulse solver to solve contacts.
this.iterations = 10;
/// Gravity to pass to rigid bodies when integrating velocity.
this.gravity = cpVect.Zero;
/// Damping rate expressed as the fraction of velocity bodies retain each second.
/// A value of 0.9 would mean that each body's velocity will drop 10% per second.
/// The default value is 1.0, meaning no damping is applied.
/// @note This damping value is different than those of cpDampedSpring and cpDampedRotarySpring.
this.damping = 1;
/// Amount of encouraged penetration between colliding shapes..
/// Used to reduce oscillating contacts and keep the collision cache warm.
/// Defaults to 0.1. If you have poor simulation quality,
/// increase this number as much as possible without allowing visible amounts of overlap.
this.collisionSlop = 0.1f;
/// Determines how fast overlapping shapes are pushed apart.
/// Expressed as a fraction of the error remaining after each second.
/// Defaults to pow(1.0 - 0.1, 60.0) meaning that Chipmunk fixes 10% of overlap each frame at 60Hz.
this.collisionBias = cp.cpfpow(1f - 0.1f, 60f);
/// Number of frames that contact information should persist.
/// Defaults to 3. There is probably never a reason to change this value.
this.collisionPersistence = 3;
this.locked = 0;
this.stamp = 0;
this.staticShapes = new cpBBTree(null);
this.dynamicShapes = new cpBBTree(this.staticShapes);
this.dynamicShapes.SetVelocityFunc(o => ShapeVelocityFunc(o as cpShape));
this.dynamicBodies = new List <cpBody>();
this.staticBodies = new List <cpBody>();
this.rousedBodies = new List <cpBody>();
this.sleepingComponents = new List <cpBody>();
/// Time a group of bodies must remain idle in order to fall asleep.
/// Enabling sleeping also implicitly enables the the contact graph.
/// The default value of Infinity disables the sleeping algorithm.
this.sleepTimeThreshold = cp.Infinity;
/// Speed threshold for a body to be considered idle.
/// The default value of 0 means to let the space guess a good threshold based on gravity.
this.idleSpeedThreshold = 0;
this.arbiters = new List <cpArbiter>();
this.cachedArbiters = new Dictionary <ulong, cpArbiter>();
this.constraints = new List <cpConstraint>();
this.usesWildcards = false;
this.defaultHandler = cpCollisionHandlerDoNothing;
this.collisionHandlers = new Dictionary <ulong, cpCollisionHandler>();
this.postStepCallbacks = new List <cpPostStepCallback>();
this.skipPostStep = false;
/// The designated static body for this space.
/// You can modify this body, or replace it with your own static body.
/// By default it points to a statically allocated cpBody in the cpSpace struct.
this.staticBody = cpBody.NewStatic();
}
/// Test if a rigid body has been added to the space.
public bool ContainsBody(cpBody body)
{
return(body.space == this);
}
