//public override void Draw(cpDebugDraw m_debugDraw)
//{
// if (m_debugDraw.Flags.HasFlag(cpDrawFlags.Shapes) || m_debugDraw.Flags.HasFlag(cpDrawFlags.All))
// {
// cpColor color = cp.GetShapeColor(this);
// int count = Count;
// m_debugDraw.DrawPolygon(GetVertices(), count, color);
// }
// if (m_debugDraw.Flags.HasFlag(cpDrawFlags.BB) || m_debugDraw.Flags.HasFlag(cpDrawFlags.All))
// bb.Draw(m_debugDraw);
//}
public static cpPolyShape BoxShape(cpBody body, float width, float height, float radius)
{
var hw = width / 2f;
var hh = height / 2f;
return BoxShape2(body, new cpBB(-hw, -hh, hw, hh), radius);
}
public cpPivotJoint(cpBody a, cpBody b, cpVect anchorA, cpVect anchorB)
: base(a, b)
{
this.anchorA = anchorA;
this.anchorB = anchorB;
this.jAcc = cpVect.Zero;
}
public cpConstraint(cpBody a, cpBody b)
{
/// The first body connected to this constraint.
this.a = a;
/// The second body connected to this constraint.
this.b = b;
this.space = null;
this.next_a = null;
this.next_b = null;
/// The maximum force that this constraint is allowed to use.
this.maxForce = cp.Infinity;
/// The rate at which joint error is corrected.
/// Defaults to pow(1 - 0.1, 60) meaning that it will
/// correct 10% of the error every 1/60th of a second.
this.errorBias = cp.cpfpow(1f - 0.1f, 60f);
/// The maximum rate at which joint error is corrected.
this.maxBias = cp.Infinity;
this.collideBodies = true;
//Not clear
preSolve = DefaultPreSolve;
postSolve = DefaultPostSolve;
}
public static void FloodFillComponent(cpBody root, cpBody body)
{
// Kinematic bodies cannot be put to sleep and prevent bodies they are touching from sleeping.
// Static bodies are effectively sleeping all the time.
if (body.bodyType == cpBodyType.DYNAMIC)
{
cpBody other_root = cp.ComponentRoot(body);
if (other_root == null)
{
cp.componentAdd(root, body);
body.eachArbiter((arb, o) =>
{
FloodFillComponent(root, (body == arb.body_a ?
arb.body_b : arb.body_a));
}, null);
body.eachConstraint((constraint, o) =>
{
FloodFillComponent(root,
(body == constraint.a ? constraint.b : constraint.a));
}, null);
}
else
{
cp.AssertSoft(other_root == root, "Internal Error: Inconsistency dectected in the contact graph.");
}
}
}
public cpSimpleMotor(cpBody a, cpBody b, float rate)
: base(a, b)
{
this.rate = rate;
this.jAcc = 0.0f;
}
public cpRotaryLimitJoint(cpBody a, cpBody b, float min, float max)
: base(a, b)
{
this.min = min;
this.max = max;
this.jAcc = 0.0f;
}
public cpGearJoint(cpBody a, cpBody b, float phase, float ratio)
: base(a, b)
{
this.phase = phase;
this.ratio = ratio;
this.ratio_inv = 1 / ratio;
this.jAcc = 0.0f;
}
public PhysicSprite(cpBody body, cpShape shape)
{
_body = body;
_shape = shape;
Active = true;
_body.SetUserData(this);
}
public cpRatchetJoint(cpBody a, cpBody b, float phase, float ratchet)
: base(a, b)
{
this.angle = 0.0f;
this.phase = phase;
this.ratchet = ratchet;
// STATIC_BODY_CHECK
this.angle = (b != null ? b.a : 0.0f) - (a != null ? a.a : 0.0f);
}
public cpSlideJoint(cpBody a, cpBody b, cpVect anchorA, cpVect anchorB, float min, float max)
: base(a, b)
{
this.anchorA = anchorA;
this.anchorB = anchorB;
this.min = min;
this.max = max;
this.jnAcc = 0.0f;
}
public void ActivateBody(cpBody body)
{
cp.AssertHard(body.bodyType == cpBodyType.DYNAMIC, "Internal error: Attempting to deactivate a non-dynamic body.");
if (this.IsLocked)
{
// cpSpaceActivateBody() is called again once the space is unlocked
if (!this.rousedBodies.Contains(body))
this.rousedBodies.Add(body);
}
else
{
cp.AssertSoft(body.nodeRoot == null &&
body.nodeNext == null, "Internal error: Activating body non-NULL node pointers.");
this.dynamicBodies.Add(body);
body.eachShape((s, o) =>
{
this.staticShapes.Remove(s.hashid);
this.dynamicShapes.Insert(s.hashid, s);
}, null);
body.eachArbiter((arb, o) =>
{
cpBody bodyA = arb.body_a;
// Arbiters are shared between two bodies that are always woken up together.
// You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter.
// The edge case is when static bodies are involved as the static bodies never actually sleep.
// If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked.
if (body == bodyA || bodyA.bodyType == cpBodyType.STATIC)
{
cpShape a = arb.a, b = arb.b;
this.cachedArbiters.Add(cp.CP_HASH_PAIR(a.hashid, b.hashid), arb);
// Update the arbiter's state
arb.stamp = this.stamp;
arb.handler = this.LookupHandler(a.type, b.type, defaultHandler);
this.arbiters.Add(arb);
}
}, null);
body.eachConstraint((constraint, o) =>
{
var bodyA = constraint.a;
if (body == bodyA || bodyA.bodyType == cpBodyType.STATIC)
this.constraints.Add(constraint);
}, null);
}
}
public static bool ComponentActive(cpBody root, float threshold)
{
//CP_BODY_FOREACH_COMPONENT
for (var body = root; body != null; body = body.nodeNext)
{
if (body.nodeIdleTime < threshold)
return true;
}
return false;
}
public CCPhysicsShapeInfo(CCPhysicsShape shape)
{
_shapes = new List<cpShape>();
// TODO: Complete member initialization
_shape = shape;
_body = _sharedBody;
_group = cp.NO_GROUP; //CP_NO_GROUP ?¿
}
public cpDampedRotarySpring(cpBody a, cpBody b, float restAngle, float stiffness, float damping)
: base(a, b)
{
this.restAngle = restAngle;
this.stiffness = stiffness;
this.damping = damping;
this.springTorqueFunc = defaultSpringTorque;
this.target_wrn = 0.0f;
this.w_coef = 0.0f;
this.iSum = 0.0f;
}
public cpPolyShape ToShape(cpBody body, float radius, cpVect size, int shapeIdx = 0)
{
var source = Shapes[shapeIdx];
var vertices = new cpVect[source.Length];
for (int i = 0, count = source.Length; i < count; i++)
{
var sourceVect = source[i];
var vect = cpVect.cpv(sourceVect.x * size.x, sourceVect.y * size.y);
vertices[i] = vect;
}
return new cpPolyShape(body, vertices.Length, vertices, radius);
}
public cpPolyShape(cpBody body, int count, cpVect[] verts,
cpTransform transform, float radius)
: base(body, new cpShapeMassInfo())
{
cpVect[] hullVerts = new cpVect[count];
// Transform the verts before building the hull in case of a negative scale.
for (int i = 0; i < count; i++)
hullVerts[i] = cpTransform.Point(transform, verts[i]);
int hullCount = cp.ConvexHull(count, hullVerts, ref hullVerts, null, 0.0f);
InitRaw(hullCount, hullVerts, radius);
}
public cpGrooveJoint(cpBody a, cpBody b, cpVect groove_a, cpVect groove_b, cpVect anchorB)
: base(a, b)
{
this.grv_a = groove_a;
this.grv_b = groove_b;
this.grv_n = cpVect.cpvperp(cpVect.cpvnormalize(cpVect.cpvsub(groove_b, groove_a)));
this.anchorB = anchorB;
this.grv_tn = null;
this.clamp = 0.0f;
this.r1 = this.r2 = null;
this.jAcc = cpVect.Zero;
this.bias = null;
}
public CCPhysicsShapeInfo(CCPhysicsShape shape)
{
_map = new Dictionary<cpShape, CCPhysicsShapeInfo>();
_shapes = new List<cpShape>();
// TODO: Complete member initialization
_shape = shape;
if (_sharedBody == null)
_sharedBody = cpBody.NewStatic(); // = cpBodyNewStatic(); ¿¿
_body = _sharedBody;
_group = cp.NO_GROUP; //CP_NO_GROUP ?¿
}
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;
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;
// compute relative rotational velocity
float wr = b.w - a.w + this.rate;
float jMax = this.maxForce * dt;
// compute normal impulse
float j = -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;
b.w += j * b.i_inv;
}
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, Action <cpShape, cpContactPointSet, object> 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 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 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 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 cpArbiterThread ThreadForBody(cpBody body)
{
//TODO: THIS NEEDS RETURN THE ORIGINAL MEMORY REFERENCE IN ARBITER
if (this.body_a == body)
return thread_a;
else
return thread_b;
}
//public static void UnthreadHelper(cpArbiter arb, cpBody body)
//{
// cpArbiterThread thread = arb.ThreadForBody(body);
// cpArbiter prev = thread.prev;
// cpArbiter next = thread.next;
// // thread_x_y is quite ugly, but it avoids making unnecessary js objects per arbiter.
// if (prev != null)
// {
// cpArbiterThread nextPrev = prev.ThreadForBody(body);
// nextPrev.next = next;
// }
// else if (body.arbiterList == arb)
// {
// // IFF prev is NULL and body->arbiterList == arb, is arb at the head of the list.
// // This function may be called for an arbiter that was never in a list.
// // In that case, we need to protect it from wiping out the body->arbiterList pointer.
// body.arbiterList = next;
// }
// if (next != null)
// {
// cpArbiterThread threadNext = next.ThreadForBody(body);
// threadNext.prev = prev;
// }
// thread.prev = null;
// thread.next = null;
//}
public static void UnthreadHelper(cpArbiter arb, cpBody body, cpArbiter prev, cpArbiter next)
{
// thread_x_y is quite ugly, but it avoids making unnecessary js objects per arbiter.
if (prev != null)
{
// cpArbiterThreadForBody(prev, body)->next = next;
if (prev.body_a == body)
{
prev.thread_a.next = next;
}
else
{
prev.thread_b.next = next;
}
}
else
{
body.arbiterList = next;
}
if (next != null)
{
// cpArbiterThreadForBody(next, body)->prev = prev;
if (next.body_a == body)
{
next.thread_a.prev = prev;
}
else
{
next.thread_b.prev = prev;
}
}
}
public virtual cpConstraint Next(cpBody body)
{
return(this.a == body ? this.next_a : this.next_b);
}
public void DrawShape(cpShape shape)
{
cpBody body = shape.body;
cpColor color = cp.GetShapeColor(shape);; // ColorForBody(body);
switch (shape.shapeType)
{
case cpShapeType.Circle:
{
cpCircleShape circle = (cpCircleShape)shape;
if (Flags.HasFlag(PhysicsDrawFlags.BB) || Flags.HasFlag(PhysicsDrawFlags.All))
{
Draw(circle.bb);
}
if (Flags.HasFlag(PhysicsDrawFlags.Shapes) || Flags.HasFlag(PhysicsDrawFlags.All))
{
Draw(circle, color);
}
}
break;
case cpShapeType.Segment:
{
cpSegmentShape seg = (cpSegmentShape)shape;
if (Flags.HasFlag(PhysicsDrawFlags.BB) || Flags.HasFlag(PhysicsDrawFlags.All))
{
Draw(seg.bb);
}
if (Flags.HasFlag(PhysicsDrawFlags.Shapes) || Flags.HasFlag(PhysicsDrawFlags.All))
{
Draw(seg, color);
}
}
break;
case cpShapeType.Polygon:
{
cpPolyShape poly = (cpPolyShape)shape;
if (Flags.HasFlag(PhysicsDrawFlags.BB) || Flags.HasFlag(PhysicsDrawFlags.All))
{
Draw(poly.bb);
}
if (Flags.HasFlag(PhysicsDrawFlags.Shapes) || Flags.HasFlag(PhysicsDrawFlags.All))
{
Draw(poly, color);
}
}
break;
default:
cp.AssertHard(false, "Bad assertion in DrawShape()");
break;
}
}
public cpCircleShape(cpBody body, float radius, cpVect offset)
: base(body, cpShapeMassInfo.cpCircleShapeMassInfo(0.0f, radius, offset))
{
this.c = offset;
this.r = radius;
this.shapeType = cpShapeType.Circle;
}
public cpShape(cpBody body, cpShapeMassInfo massInfo)
{
/// The rigid body this collision shape is attached to.
this.body = body;
this.massInfo = massInfo;
/// The current bounding box of the shape.
/// The current bounding box of the shape.
///
//this.bb_l = this.bb_b = this.bb_r = this.bb_t = 0;
this.bb = new cpBB(0, 0, 0, 0);
//this.hashid = (cp.shapeIDCounter++).ToString();
/// Sensor flag.
/// Sensor shapes call collision callbacks but don't produce collisions.
this.sensor = false;
filter = new cpShapeFilter(cp.NO_GROUP, cp.ALL_CATEGORIES, cp.ALL_CATEGORIES);
/// Coefficient of restitution. (elasticity)
this.e = 0;
/// Coefficient of friction.
this.u = 0;
/// Surface velocity used when solving for friction.
this.surfaceV = cpVect.Zero;
/// Collision type of this shape used when picking collision handlers.
this.type = 0;
this.space = null;
}
public cpArbiter Next(cpBody body)
{
var next = (this.body_a == body ? this.thread_a.next : this.thread_b.next);
return(next == this ? null : next);
}
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);
}
/// Test if a rigid body has been added to the space.
public bool ContainsBody(cpBody body)
{
return(body.space == this);
}
public static float SetAngle(cpBody body, float angle)
{
body.a = angle;
body.AssertSaneBody();
return(angle);
}
public static cpBody ComponentRoot(cpBody body)
{
return(body != null ? body.nodeRoot : null);
}
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 cpArbiter Next(cpBody body)
{
var next = (this.body_a == body ? this.thread_a.next : this.thread_b.next);
return next == this ? null : next;
}
public cpComponentNode(cpBody root, cpBody next, float idleTime)
{
this.root = root;
this.next = next;
this.idleTime = idleTime;
}
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 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;
}
}
}
/// Allocate and initialize a cpBody.
public static cpBody New(float mass, float moment)
{
cpBody tmp = new cpBody(mass, moment);
return(tmp);
}
public cpPolyShape(cpBody body, int count, cpVect[] verts, float radius)
: base(body, new cpShapeMassInfo())
{
InitRaw(count, verts, radius);
}
public void GetBodies(out cpBody a, out cpBody b)
{
cpShape shape_a, shape_b;
GetShapes(out shape_a, out shape_b);
a = shape_a.body;
b = shape_b.body;
}
public static cpPolyShape BoxShape2(cpBody body, cpBB box, float radius)
{
cpVect[] verts = new cpVect[] {
new cpVect(box.r, box.b),
new cpVect(box.r, box.t),
new cpVect(box.l, box.t),
new cpVect(box.l, box.b),
};
return new cpPolyShape(body, 4, verts, radius);
}
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.ToList();
//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 cpSegmentShape(cpBody body, cpVect a, cpVect b, float r)
: base(body, cpShapeMassInfo.cpSegmentShapeMassInfo(0.0f, a, b, r))
{
this.a = a;
this.b = b;
this.n = cpVect.cpvrperp(cpVect.vnormalize(cpVect.cpvsub(b, a)));
this.r = r;
this.a_tangent = cpVect.Zero;
this.b_tangent = cpVect.Zero;
this.shapeType = cpShapeType.Segment;
}
public static cpArbiterThread ThreadForBody(cpArbiter arb, cpBody body)
{
return(arb.body_a == body ? arb.thread_a : arb.thread_b);
}
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;
}
public cpArbiter Next(cpBody body)
{
return(this.body_a == body ? this.thread_a.next : this.thread_b.next);
}
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 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 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.ToList();
//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;
}