/// Remove a collision shape from the simulation.
public void RemoveShape(cpShape shape)
{
var body = shape.body;
cp.AssertHard(ContainsShape(shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)");
cp.AssertSpaceUnlocked(this);
bool isStatic = body.bodyType == cpBodyType.STATIC;
if (isStatic)
{
body.ActivateStatic(shape);
}
else
{
body.Activate();
}
body.RemoveShape(shape);
this.FilterArbiters(body, shape);
(isStatic ? this.staticShapes : this.dynamicShapes).Remove(shape.hashid);
shape.space = null;
shape.hashid = 0;
}
public static cpColor GetShapeColor(cpShape shape)
{
if (shape.sensor)
{
return(new cpColor(128, 128, 128));
}
else
{
if (shape.body.IsSleeping())
{
return(new cpColor(125, 125, 125));
}
else if (shape.body.nodeIdleTime > shape.space.sleepTimeThreshold)
{
return(new cpColor(170, 170, 170));
}
else if (shape.body.bodyType == cpBodyType.STATIC)
{
return(new cpColor(75, 75, 75));
}
else
{
return(styles[(int)shape.hashid % styles.Count]);
}
}
}
/// Add a collision shape to the simulation.
/// If the shape is attached to a static body, it will be added as a static shape.
public cpShape AddShape(cpShape shape)
{
var body = shape.body;
cp.AssertHard(shape.space != this, "You have already added this shape to this space. You must not add it a second time.");
cp.AssertHard(shape.space == null, "You have already added this shape to another space. You cannot add it to a second.");
cp.AssertSpaceUnlocked(this);
bool isStatic = body.bodyType == cpBodyType.STATIC;
if (!isStatic)
{
body.Activate();
}
body.AddShape(shape);
shape.hashid = cp.shapeIDCounter++;
shape.Update(body.transform);
(isStatic ? this.staticShapes : this.dynamicShapes).Insert(shape.hashid, shape);
shape.space = this;
return(shape);
}
public void eachShape(cpBodyShapeIteratorFunc func, object data)
{
for (cpShape var = this.shapeList; var != null; var = var.next)
{
func(var, data);
}
}
/// Test if a point lies within a shape.
public static cpContactPointSet Collide(cpShape a, cpShape b, ref List <cpContact> contacts)
{
//cpContact[] contacts = new cpContact[cpArbiter.CP_MAX_CONTACTS_PER_ARBITER];
cpCollisionInfo info = cpCollision.cpCollide(a, b, 0, ref contacts);
cpContactPointSet set = new cpContactPointSet();
set.count = info.count;
set.points = new PointsDistance[set.count];
// cpCollideShapes() may have swapped the contact order. Flip the normal.
bool swapped = (a != info.a);
set.normal = (swapped ? cpVect.cpvneg(info.n) : info.n);
for (int i = 0; i < info.count; i++)
{
cpVect p1 = contacts[i].r1;
cpVect p2 = contacts[i].r2;
set.points[i] = new PointsDistance();
set.points[i].pointA = (swapped ? p2 : p1);
set.points[i].pointB = (swapped ? p1 : p2);
set.points[i].distance = cpVect.cpvdot(cpVect.cpvsub(p2, p1), set.normal);
}
return(set);
}
public SupportContext(cpShape shape1, cpShape shape2, SupportPointFunc func1, SupportPointFunc func2)
{
this.shape1 = shape1;
this.shape2 = shape2;
this.func1 = func1;
this.func2 = func2;
}
// Equal function for arbiterSet.
public static bool SetEql(cpShape[] shapes, cpArbiter arb)
{
cpShape a = shapes[0];
cpShape b = shapes[1];
return((a == arb.a && b == arb.b) || (b == arb.a && a == arb.b));
}
public static void CircleSegmentQuery(cpShape shape, cpVect center, float r1, cpVect a, cpVect b, float r2, ref cpSegmentQueryInfo info)
{
// offset the line to be relative to the circle
cpVect da = cpVect.cpvsub(a, center);
cpVect db = cpVect.cpvsub(b, center);
float rsum = r1 + r2;
float qa = cpVect.cpvdot(da, da) - 2 * cpVect.cpvdot(da, db) + cpVect.cpvdot(db, db);
float qb = cpVect.cpvdot(da, db) - cpVect.cpvdot(da, da);
float det = qb * qb - qa * (cpVect.cpvdot(da, da) - rsum * rsum);
if (det >= 0.0f)
{
float t = (-qb - cp.cpfsqrt(det)) / (qa);
if (0.0f <= t && t <= 1.0f)
{
{
cpVect n = cpVect.cpvnormalize(cpVect.cpvlerp(da, db, t));
info.shape = shape;
info.point = cpVect.cpvsub(cpVect.cpvlerp(da, db, t), cpVect.cpvmult(n, r2));
info.normal = n;
info.alpha = t;
}
}
}
}
/// A colliding pair of shapes.
public cpArbiter(cpShape a, cpShape b)
{
this.handler = null;
this.swapped = false;
this.handlerA = null;
this.handlerB = null;
/// Calculated value to use for the elasticity coefficient.
/// Override in a pre-solve collision handler for custom behavior.
this.e = 0;
/// Calculated value to use for the friction coefficient.
/// Override in a pre-solve collision handler for custom behavior.
this.u = 0;
/// Calculated value to use for applying surface velocities.
/// Override in a pre-solve collision handler for custom behavior.
this.surface_vr = cpVect.Zero;
this.a = a; this.body_a = a.body;
this.b = b; this.body_b = b.body;
this.thread_a = new cpArbiterThread(null, null);
this.thread_b = new cpArbiterThread(null, null);
this.contacts = new List <cpContact>();
this.stamp = 0;
this.state = cpArbiterState.FirstCollision;
}
public void FilterArbiters(cpBody body, cpShape filter)
{
List <ulong> safeDelete = new List <ulong>();
foreach (var hash in this.cachedArbiters)
{
cpArbiter arb = hash.Value;
// Match on the filter shape, or if it's null the filter body
if (
(body == arb.body_a && (filter == arb.a || filter == null)) ||
(body == arb.body_b && (filter == arb.b || filter == null))
)
{
// Call separate when removing shapes.
if (filter != null && arb.state != cpArbiterState.Cached)
{
arb.state = cpArbiterState.Invalidated;
cpCollisionHandler handler = arb.handler;
handler.separateFunc(arb, this, handler.userData);
}
arb.Unthread();
this.arbiters.Remove(arb);
safeDelete.Add(hash.Key);
}
}
foreach (var item in safeDelete)
{
cachedArbiters.Remove(item);
}
}
public void RemoveShape(cpShape shape)
{
cpShape prev = shape.prev;
cpShape next = shape.next;
if (prev != null)
{
prev.next = next;
}
else
{
this.shapeList = next;
}
if (next != null)
{
next.prev = prev;
}
shape.prev = null;
shape.next = null;
if (bodyType == cpBodyType.DYNAMIC && shape.massInfo.m > 0.0f)
{
AccumulateMassFromShapes();
}
}
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 cpPointQueryExtendedInfo(cpShape tShape)
{
/// The nearest shape, NULL if no shape was within range.
this.shape = tShape;
/// The closest point on the shape's surface. (in world space coordinates)
this.d = cp.Infinity;
/// The distance to the point. The distance is negative if the point is inside the shape.
this.n = cpVect.Zero;
}
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 cpCollisionInfo(cpShape a, cpShape b, ulong id, cpVect n, List <cpContact> contacts)
{
// TODO: Complete member initialization
this.a = a;
this.b = b;
this.id = id;
this.n = n;
this.arr = contacts;
}
protected override void OnStart()
{
base.OnStart();
XSpaceManager.Instance.Space.SetGravity(new cpVect(0.0f, -Parameters.Gravity));
mPlayerBody = GetComponent <XRigidbody2D>().Rigidbody2D;
mPlayerShape = GetComponent <XCollider2D>().Collider2D;
mPlayerBody.SetVelocityUpdateFunc(PlayerUpdateVelocity);
mCharacter = GetComponent <XCharacter>();
}
/// Return the colliding shapes involved for this arbiter.
/// The order of their cpSpace.collision_type values will match
/// the order set when the collision handler was registered.
public void GetShapes(out cpShape a, out cpShape b)
{
if (swapped)
{
a = this.b; b = this.a;
}
else
{
a = this.a;
} b = this.b;
}
public cpSegmentQueryInfo(cpShape shape, cpVect point, cpVect normal, float alpha)
{
/// The shape that was hit, NULL if no collision occured.
this.shape = shape;
/// The normalized distance along the query segment in the range [0, 1].
this.alpha = alpha;
/// The normal of the surface hit.
this.normal = normal;
this.point = point;
}
public void Set(cpSegmentQueryInfo info1)
{
/// The shape that was hit, NULL if no collision occured.
this.shape = info1.shape;
/// The normalized distance along the query segment in the range [0, 1].
this.alpha = info1.alpha;
/// The normal of the surface hit.
this.normal = info1.normal;
this.point = info1.point;
}
public cpPointQueryInfo(cpShape shape, cpVect point, float distance, cpVect gradient)
{
/// The nearest shape, NULL if no shape was within range.
this.shape = shape;
/// The closest point on the shape's surface. (in world space coordinates)
this.point = point;
/// The distance to the point. The distance is negative if the point is inside the shape.
this.distance = distance;
this.gradient = gradient;
}
//MARK: BB Query Functions
public ulong BBQueryFunc(BBQueryContext context, cpShape shape, ulong id, object data)
{
if (
!cpShapeFilter.Reject(shape.filter, context.filter) &&
context.bb.Intersects(shape.bb)
)
{
context.func(shape, data);
}
return(id);
}
/// Update the collision detection data for a specific shape in the space.
public void ReindexShape(cpShape shape)
{
cp.AssertHard(!IsLocked, "You cannot manually reindex objects while the space is locked. Wait until the current query or step is complete.");
//var body = shape.body;
shape.CacheBB();
//shape.Update(body.GetPosition(), body.GetRotation());
// attempt to rehash the shape in both hashes
this.dynamicShapes.ReindexObject(shape, shape.hashid);
this.staticShapes.ReindexObject(shape, shape.hashid);
}
public static bool QueryReject(cpShape a, cpShape b)
{
return(
// BBoxes must overlap
!a.bb.Intersects(b.bb)
// Don't collide shapes attached to the same body.
|| a.body == b.body
// Don't collide shapes that are filtered.
|| a.filter.Reject(b.filter)
// Don't collide bodies if they have a constraint with collideBodies == cpFalse.
|| QueryRejectConstraint(a.body, b.body)
);
}
// Wake up any sleeping or idle bodies touching a static body.
public void ActivateStatic(cpShape filter)
{
cp.AssertHard(bodyType == cpBodyType.STATIC, "Body.activateStatic() called on a non-static body.");
eachArbiter((arb, o) =>
{
if (filter == null || filter == arb.a || filter == arb.b)
{
(arb.body_a == this ? arb.body_b : arb.body_a).Activate();
}
}, null);
// TODO should also activate joints!
}
//MARK: Segment Query Functions
public float SegmentQueryFunc(SegmentQueryContext context, cpShape shape, object data)
{
cpSegmentQueryInfo info = null;
if (
!cpShapeFilter.Reject(shape.filter, context.filter) &&
shape.SegmentQuery(context.start, context.end, context.radius, ref info)
)
{
context.func(shape, info.point, info.alpha, info.normal, data);
}
return(1.0f);
}
public float SegmentQueryFirstFunc(SegmentQueryContext context, cpShape shape, cpSegmentQueryInfo output)
{
cpSegmentQueryInfo info = null;
if (
!cpShapeFilter.Reject(shape.filter, context.filter) && !shape.sensor &&
shape.SegmentQuery(context.start, context.end, context.radius, ref info) &&
info.alpha < output.alpha
)
{
output = info;
}
return(output.alpha);
}
public ulong NearestPointQuery(PointQueryContext context, cpShape shape, ulong id, object data)
{
if (
!cpShapeFilter.Reject(shape.filter, context.filter)
)
{
cpPointQueryInfo info = null;
shape.PointQuery(context.point, ref info);
if (info.shape != null && info.distance < context.maxDistance)
{
context.func(shape, info.point, info.distance, info.gradient, data);
}
}
return(id);
}
public void AddShape(cpShape shape)
{
//this.shapeList.Add(shape);
cpShape next = this.shapeList;
if (next != null)
{
next.prev = shape;
}
shape.next = next;
this.shapeList = shape;
if (shape.massInfo.m > 0.0f)
{
AccumulateMassFromShapes();
}
}
/// <summary>
/// CREATES A BODY WITH MASS AND INERTIA
/// </summary>
/// <param name="mass"></param>
/// <param name="moment"></param>
public cpBody(float mass, float moment)
{
transform = new cpTransform();
this.cog = cpVect.Zero;
this.space = null;
this.shapeList = null;
this.arbiterList = null; // These are both wacky linked lists.
this.constraintList = null;
velocity_func = UpdateVelocity;
position_func = UpdatePosition;
// This stuff is used to track information on the collision graph.
this.nodeRoot = null;
this.nodeNext = null;
this.nodeIdleTime = 0;
/// Position of the rigid body's center of gravity.
this.p = cpVect.Zero;
/// Velocity of the rigid body's center of gravity.
this.v = cpVect.Zero;
/// Force acting on the rigid body's center of gravity.
this.f = cpVect.Zero;
/// Angular velocity of the body around it's center of gravity in radians/second.
this.w = 0;
/// Torque applied to the body around it's center of gravity.
this.t = 0;
// This stuff is all private.
this.v_bias = cpVect.Zero; //x = this.v_biasy = 0;
this.w_bias = 0;
this.userData = null;
this.SetMass(mass);
this.SetMoment(moment);
this.SetAngle(0.0f);
}
public ulong NearestPointQueryNearest(object ctx, cpShape shape, ulong id, ref object outp)
{
PointQueryContext context = (PointQueryContext)ctx;
cpPointQueryInfo output = (cpPointQueryInfo)outp;
if (
!cpShapeFilter.Reject(shape.filter, context.filter) && !shape.sensor
)
{
cpPointQueryInfo info = null;
shape.PointQuery(context.point, ref info);
if (info.distance < output.distance)
{
outp = (object)info;
}
}
return(id);
}
