static void
applyImpulse(cpPivotJoint *joint)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
cpVect r1 = joint->r1;
cpVect r2 = joint->r2;
// compute relative velocity
cpVect vr = relative_velocity(a, b, r1, r2);
// compute normal impulse
cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2);
cpVect jOld = joint->jAcc;
joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->jMaxLen);
j = cpvsub(joint->jAcc, jOld);
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, j);
}
void
cpArbiterApplyCachedImpulse(cpArbiter *arb)
{
cpShape *shapea = arb->a;
cpShape *shapeb = arb->b;
arb->u = shapea->u * shapeb->u;
arb->target_v = cpvsub(shapeb->surface_v, shapea->surface_v);
cpBody *a = shapea->body;
cpBody *b = shapeb->body;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
cpVect t = cpvperp(con->n);
cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc));
cpBodyApplyImpulse(a, cpvneg(j), con->r1);
cpBodyApplyImpulse(b, j, con->r2);
}
}
static void
applyImpulse(cpPivotJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
cpVect r1 = joint->r1;
cpVect r2 = joint->r2;
// compute relative velocity
cpVect vr = relative_velocity(a, b, r1, r2);
// compute normal impulse
cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
cpVect jOld = joint->jAcc;
joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt);
j = cpvsub(joint->jAcc, jOld);
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, j);
}
// Add contact points for circle to circle collisions.
// Used by several collision tests.
static int
circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpContact *con)
{
cpFloat mindist = r1 + r2;
cpVect delta = cpvsub(p2, p1);
cpFloat distsq = cpvlengthsq(delta);
if(distsq >= mindist*mindist) return 0;
cpFloat dist = cpfsqrt(distsq);
// Allocate and initialize the contact.
cpContactInit(
con,
cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/(dist ? dist : INFINITY))),
(dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)),
dist - mindist,
0
);
return 1;
}
int MagneticGripperGrippableCollisionPreSolve(cpArbiter* pt_arb, cpSpace* pt_space, void* p_data) {
/* Get the shapes involved */
CP_ARBITER_GET_SHAPES(pt_arb, ptGripperShape, ptGrippableShape);
/* Get a reference to the gripper data */
SDynamics2DEngineGripperData& sGripperData = *reinterpret_cast<SDynamics2DEngineGripperData*>(ptGripperShape->data);
/* The gripper is locked or unlocked? */
if(sGripperData.GripperEntity.IsUnlocked()) {
/* The gripper is locked. If it was gripping an object,
* release it. Then, process the collision normally */
if(sGripperData.GripperEntity.IsGripping()) {
sGripperData.ClearConstraints();
}
return 1;
}
else if(! sGripperData.GripperEntity.IsGripping()) {
/* The gripper is unlocked and free, create the joints */
/* Prevent gripper from slipping */
pt_arb->e = 0.0f; // No elasticity
pt_arb->u = 1.0f; // Max friction
pt_arb->surface_vr = cpvzero; // No surface velocity
/* Calculate the anchor point on the grippable body
as the centroid of the contact points */
cpVect tGrippableAnchor = cpvzero;
for(SInt32 i = 0; i < pt_arb->numContacts; ++i) {
tGrippableAnchor = cpvadd(tGrippableAnchor, cpArbiterGetPoint(pt_arb, i));
}
tGrippableAnchor = cpvmult(tGrippableAnchor, 1.0f / pt_arb->numContacts);
/* Create a constraint */
sGripperData.GripConstraint = cpSpaceAddConstraint(pt_space,
cpPivotJointNew(
ptGripperShape->body,
ptGrippableShape->body,
tGrippableAnchor));
sGripperData.GripConstraint->biasCoef = 0.95f; // Correct overlap
sGripperData.GripConstraint->maxBias = 0.01f; // Max correction speed
sGripperData.GripperEntity.SetGrippedEntity(*reinterpret_cast<CEmbodiedEntity*>(ptGrippableShape->data));
}
/* Ignore the collision, the objects are gripped already */
return 0;
}
static void
cpPolyShapeNearestPointQuery(cpPolyShape *poly, cpVect p, cpNearestPointQueryInfo *info){
int count = poly->numVerts;
cpSplittingPlane *planes = poly->tPlanes;
cpVect *verts = poly->tVerts;
cpFloat r = poly->r;
cpVect v0 = verts[count - 1];
cpFloat minDist = INFINITY;
cpVect closestPoint = cpvzero;
cpVect closestNormal = cpvzero;
cpBool outside = cpFalse;
for(int i=0; i<count; i++){
if(cpSplittingPlaneCompare(planes[i], p) > 0.0f) outside = cpTrue;
cpVect v1 = verts[i];
cpVect closest = cpClosetPointOnSegment(p, v0, v1);
cpFloat dist = cpvdist(p, closest);
if(dist < minDist){
minDist = dist;
closestPoint = closest;
closestNormal = planes[i].n;
}
v0 = v1;
}
cpFloat dist = (outside ? minDist : -minDist);
cpVect g = cpvmult(cpvsub(p, closestPoint), 1.0f/dist);
info->shape = (cpShape *)poly;
info->p = cpvadd(closestPoint, cpvmult(g, r));
info->d = dist - r;
// Use the normal of the closest segment if the distance is small.
info->g = (minDist > MAGIC_EPSILON ? g : closestNormal);
}
static void DrawShape(cpShape *shape, DrawNode *renderer)
{
cpBody *body = cpShapeGetBody(shape);
Color4F color = ColorForBody(body);
switch (shape->CP_PRIVATE(klass)->type)
{
case CP_CIRCLE_SHAPE:
{
cpCircleShape *circle = (cpCircleShape *)shape;
cpVect center = circle->tc;
cpFloat radius = circle->r;
renderer->drawDot(cpVert2Point(center), cpfmax(radius, 1.0), color);
renderer->drawSegment(cpVert2Point(center), cpVert2Point(cpvadd(center, cpvmult(cpBodyGetRotation(body), radius))), 1.0, color);
}
break;
case CP_SEGMENT_SHAPE:
{
cpSegmentShape *seg = (cpSegmentShape *)shape;
renderer->drawSegment(cpVert2Point(seg->ta), cpVert2Point(seg->tb), cpfmax(seg->r, 2.0), color);
}
break;
case CP_POLY_SHAPE:
{
cpPolyShape* poly = (cpPolyShape*)shape;
Color4F line = color;
line.a = cpflerp(color.a, 1.0, 0.5);
int num = poly->count;
Vec2* pPoints = new (std::nothrow) Vec2[num];
for(int i=0;i<num;++i)
pPoints[i] = cpVert2Point(poly->planes[i].v0);
renderer->drawPolygon(pPoints, num, color, 1.0, line);
CC_SAFE_DELETE_ARRAY(pPoints);
}
break;
default:
cpAssertHard(false, "Bad assertion in DrawShape()");
}
}
static cpBody *
addChassis(cpVect pos, cpVect boxOffset)
{
int num = 4;
cpVect verts[] = {
cpv(-40,-15),
cpv(-40, 15),
cpv( 40, 15),
cpv( 40,-15),
};
cpFloat mass = 5.0f;
cpBody *body = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForPoly(mass, num, verts, cpvzero)));
body->p = cpvadd(pos, boxOffset);
cpShape *shape = cpSpaceAddShape(space, cpPolyShapeNew(body, num, verts, cpvzero));
shape->e = 0.0f; shape->u = 0.7f;
shape->group = 1; // use a group to keep the car parts from colliding
return body;
}
static cpBB
cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot)
{
cpVect *src = poly->verts;
cpVect *dst = poly->tVerts;
cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY;
cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY;
for(int i=0; i<poly->numVerts; i++){
cpVect v = cpvadd(p, cpvrotate(src[i], rot));
dst[i] = v;
l = cpfmin(l, v.x);
r = cpfmax(r, v.x);
b = cpfmin(b, v.y);
t = cpfmax(t, v.y);
}
cpFloat radius = poly->r;
return cpBBNew(l - radius, b - radius, r + radius, t + radius);
}
bool Player::update(float time, int direction) {
this->direction = direction;
loops++;
if(direction & UP){
jumpState = 1;
}else{
jumpState = 0;
}
if(jumpState && !lastJumpState && grounded){
cpFloat jump_v = cpfsqrt(2.0*JUMP_HEIGHT*GRAVITY);
playerBody->v = cpvadd(playerBody->v, cpv(0.0, -jump_v));
remainingBoost = JUMP_BOOST_HEIGHT/jump_v;
}
remainingBoost -= 1.0f/60.0f;
lastJumpState = jumpState;
return true;
}
static void
update(int ticks)
{
messageString[0] = '\0';
cpVect start = cpvzero;
cpVect end = /*cpv(0, 85);//*/mousePoint;
cpVect lineEnd = end;
{
char infoString[1024];
sprintf(infoString, "Query: Dist(%f) Point%s, ", cpvdist(start, end), cpvstr(end));
strcat(messageString, infoString);
}
cpSegmentQueryInfo info = {};
if(cpSpaceSegmentQueryFirst(space, start, end, CP_ALL_LAYERS, CP_NO_GROUP, &info)){
cpVect point = cpSegmentQueryHitPoint(start, end, info);
lineEnd = cpvadd(point, cpvzero);//cpvmult(info.n, 4.0f));
char infoString[1024];
sprintf(infoString, "Segment Query: Dist(%f) Normal%s", cpSegmentQueryHitDist(start, end, info), cpvstr(info.n));
strcat(messageString, infoString);
} else {
strcat(messageString, "Segment Query (None)");
}
cpSegmentShapeSetEndpoints(querySeg, start, lineEnd);
cpShapeCacheBB(querySeg); // force it to update it's collision detection data so it will draw
// normal other stuff.
int steps = 1;
cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
for(int i=0; i<steps; i++){
cpSpaceStep(space, dt);
}
}
static void
cpPolyShapePointQuery(cpPolyShape *poly, cpVect p, cpPointQueryInfo *info){
int count = poly->count;
struct cpSplittingPlane *planes = poly->planes;
cpFloat r = poly->r;
cpVect v0 = planes[count - 1].v0;
cpFloat minDist = INFINITY;
cpVect closestPoint = cpvzero;
cpVect closestNormal = cpvzero;
cpBool outside = cpFalse;
for(int i=0; i<count; i++){
cpVect v1 = planes[i].v0;
outside = outside || (cpvdot(planes[i].n, cpvsub(p,v1)) > 0.0f);
cpVect closest = cpClosetPointOnSegment(p, v0, v1);
cpFloat dist = cpvdist(p, closest);
if(dist < minDist){
minDist = dist;
closestPoint = closest;
closestNormal = planes[i].n;
}
v0 = v1;
}
cpFloat dist = (outside ? minDist : -minDist);
cpVect g = cpvmult(cpvsub(p, closestPoint), 1.0f/dist);
info->shape = (cpShape *)poly;
info->point = cpvadd(closestPoint, cpvmult(g, r));
info->distance = dist - r;
// Use the normal of the closest segment if the distance is small.
info->gradient = (minDist > MAGIC_EPSILON ? g : closestNormal);
}
// Add contact points for circle to circle collisions.
// Used by several collision tests.
static int
circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con)
{
cpFloat mindist = r1 + r2;
cpVect delta = cpvsub(p2, p1);
cpFloat distsq = cpvlengthsq(delta);
if(distsq >= mindist*mindist) return 0;
cpFloat dist = cpfsqrt(distsq);
// To avoid singularities, do nothing in the case of dist = 0.
cpFloat non_zero_dist = (dist ? dist : INFINITY);
// Allocate and initialize the contact.
cpContactInit(
con,
cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/non_zero_dist)),
cpvmult(delta, 1.0f/non_zero_dist),
dist - mindist,
0
);
return 1;
}
static void
update(int ticks)
{
static int lastJumpState = 0;
int jumpState = (arrowDirection.y > 0.0f);
cpVect groundNormal = playerInstance.groundNormal;
if(groundNormal.y > 0.0f){
playerInstance.shape->surface_v = cpvmult(cpvperp(groundNormal), 400.0f*arrowDirection.x);
} else {
playerInstance.shape->surface_v = cpvzero;
}
cpBody *body = playerInstance.shape->body;
// apply jump
if(jumpState && !lastJumpState && cpvlengthsq(groundNormal)){
// body->v = cpvmult(cpvslerp(groundNormal, cpv(0.0f, 1.0f), 0.5f), 500.0f);
body->v = cpvadd(body->v, cpvmult(cpvslerp(groundNormal, cpv(0.0f, 1.0f), 0.75f), 500.0f));
}
if(playerInstance.groundShapes->num == 0){
cpFloat air_accel = body->v.x + arrowDirection.x*(2000.0f);
body->f.x = body->m*air_accel;
// body->v.x = cpflerpconst(body->v.x, 400.0f*arrowDirection.x, 2000.0f/60.0f);
}
int steps = 3;
cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
playerInstance.groundNormal = cpvzero;
for(int i=0; i<steps; i++){
cpSpaceStep(space, dt);
}
lastJumpState = jumpState;
}
static void
update(cpSpace *space)
{
int jumpState = (ChipmunkDemoKeyboard.y > 0.0f);
// If the jump key was just pressed this frame, jump!
if(jumpState && !lastJumpState && grounded){
cpFloat jump_v = cpfsqrt(2.0*JUMP_HEIGHT*GRAVITY);
playerBody->v = cpvadd(playerBody->v, cpv(0.0, jump_v));
remainingBoost = JUMP_BOOST_HEIGHT/jump_v;
}
// Step the space
int steps = 3;
cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
for(int i=0; i<steps; i++){
cpSpaceStep(space, dt);
remainingBoost -= dt;
lastJumpState = jumpState;
}
}
static void DrawShape(cpShape *shape, DrawNode *renderer)
{
cpBody *body = shape->body;
Color4F color = ColorForBody(body);
switch (shape->CP_PRIVATE(klass)->type)
{
case CP_CIRCLE_SHAPE:
{
cpCircleShape *circle = (cpCircleShape *)shape;
cpVect center = circle->tc;
cpFloat radius = circle->r;
renderer->drawDot(cpVert2Point(center), cpfmax(radius, 1.0), color);
renderer->drawSegment(cpVert2Point(center), cpVert2Point(cpvadd(center, cpvmult(body->rot, radius))), 1.0, color);
}
break;
case CP_SEGMENT_SHAPE:
{
cpSegmentShape *seg = (cpSegmentShape *)shape;
renderer->drawSegment(cpVert2Point(seg->ta), cpVert2Point(seg->tb), cpfmax(seg->r, 2.0), color);
}
break;
case CP_POLY_SHAPE:
{
cpPolyShape *poly = (cpPolyShape *)shape;
Color4F line = color;
line.a = cpflerp(color.a, 1.0, 0.5);
Vec2* pPoints = cpVertArray2ccpArrayN(poly->tVerts, poly->numVerts);
renderer->drawPolygon(pPoints, poly->numVerts, color, 1.0, line);
CC_SAFE_DELETE_ARRAY(pPoints);
}
break;
default:
cpAssertHard(false, "Bad assertion in DrawShape()");
}
}
static void
cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p, cpPointQueryExtendedInfo *info){
if(!cpBBContainsVect(seg->shape.bb, p)) return;
cpVect a = seg->ta;
cpVect b = seg->tb;
cpVect seg_delta = cpvsub(b, a);
cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(p, a))/cpvlengthsq(seg_delta));
cpVect closest = cpvadd(a, cpvmult(seg_delta, closest_t));
cpVect delta = cpvsub(p, closest);
cpFloat distsq = cpvlengthsq(delta);
cpFloat r = seg->r;
if(distsq < r*r){
info->shape = (cpShape *)seg;
cpFloat dist = cpfsqrt(distsq);
info->d = r - dist;
info->n = cpvmult(delta, 1.0/dist);
}
}
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt, cpFloat slop, cpFloat bias)
{
cpBody *a = arb->body_a;
cpBody *b = arb->body_b;
cpVect n = arb->n;
cpVect body_delta = cpvsub(b->p, a->p);
for(int i=0; i<arb->count; i++){
struct cpContact *con = &arb->contacts[i];
// Calculate the mass normal and mass tangent.
con->nMass = 1.0f/k_scalar(a, b, con->r1, con->r2, n);
con->tMass = 1.0f/k_scalar(a, b, con->r1, con->r2, cpvperp(n));
// Calculate the target bias velocity.
cpFloat dist = cpvdot(cpvadd(cpvsub(con->r2, con->r1), body_delta), n);
con->bias = -bias*cpfmin(0.0f, dist + slop)/dt;
con->jBias = 0.0f;
// Calculate the target bounce velocity.
con->bounce = normal_relative_velocity(a, b, con->r1, con->r2, n)*arb->e;
}
}
cpVect * bmx_cpvect_add(cpVect * vec, cpVect * vec2) {
return bmx_cpvect_new(cpvadd(*vec, *vec2));
}
// Basically the same as cpvlerp(), except t = [-1, 1]
static inline cpVect
LerpT(const cpVect a, const cpVect b, const cpFloat t)
{
cpFloat ht = 0.5f*t;
return cpvadd(cpvmult(a, 0.5f - ht), cpvmult(b, 0.5f + ht));
}
// Find the closest p(t) to (0, 0) where p(t) = a*(1-t)/2 + b*(1+t)/2
// The range for t is [-1, 1] to avoid floating point issues if the parameters are swapped.
static inline cpFloat
ClosestT(const cpVect a, const cpVect b)
{
cpVect delta = cpvsub(b, a);
return -cpfclamp(cpvdot(delta, cpvadd(a, b))/cpvlengthsq(delta), -1.0f, 1.0f);
}
static void
ScaleIterator(cpBody *body, cpArbiter *arb, cpVect *sum)
{
(*sum) = cpvadd(*sum, cpArbiterTotalImpulse(arb));
}
static cpBB
cpCircleShapeCacheData(cpCircleShape *circle, cpVect p, cpVect rot)
{
cpVect c = circle->tc = cpvadd(p, cpvrotate(circle->c, rot));
return cpBBNewForCircle(c, circle->r);
}
void
cpBodyApplyForce(cpBody *body, cpVect f, cpVect r)
{
body->f = cpvadd(body->f, f);
body->t += cpvcross(r, f);
}
void
cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
{
body->v = cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt));
body->w = body->w*damping + body->t*body->i_inv*dt;
}
void Player::rightPressCheck(cpVect & moveVect) {
if (Rpressed) {
mVel = vectorForward();
moveVect = cpvadd(moveVect, vect(PLAYER_VEL, cpBodyGetAngle(mEntity->body())));
}
}
// Recursive implementation of the EPA loop.
// Each recursion adds a point to the convex hull until it's known that we have the closest point on the surface.
static struct ClosestPoints
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
{
int mini = 0;
cpFloat minDist = INFINITY;
// TODO: precalculate this when building the hull and save a step.
// Find the closest segment hull[i] and hull[i + 1] to (0, 0)
for(int j=0, i=count-1; j<count; i=j, j++) {
cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
if(d < minDist) {
minDist = d;
mini = i;
}
}
struct MinkowskiPoint v0 = hull[mini];
struct MinkowskiPoint v1 = hull[(mini + 1)%count];
cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
// Check if there is a point on the minkowski difference beyond this edge.
struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
#if DRAW_EPA
cpVect verts[count];
for(int i=0; i<count; i++) verts[i] = hull[i].ab;
ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
#endif
// The signed area of the triangle [v0.ab, v1.ab, p] will be positive if p lies beyond v0.ab, v1.ab.
cpFloat area2x = cpvcross(cpvsub(v1.ab, v0.ab), cpvadd(cpvsub(p.ab, v0.ab), cpvsub(p.ab, v1.ab)));
if(area2x > 0.0f && iteration < MAX_EPA_ITERATIONS) {
// Rebuild the convex hull by inserting p.
struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
int count2 = 1;
hull2[0] = p;
for(int i=0; i<count; i++) {
int index = (mini + 1 + i)%count;
cpVect h0 = hull2[count2 - 1].ab;
cpVect h1 = hull[index].ab;
cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
// TODO: Should this be changed to an area2x check?
if(cpvcross(cpvsub(h2, h0), cpvsub(h1, h0)) > 0.0f) {
hull2[count2] = hull[index];
count2++;
}
}
return EPARecurse(ctx, count2, hull2, iteration + 1);
} else {
// Could not find a new point to insert, so we have found the closest edge of the minkowski difference.
cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
}
void PhysicsDebugDraw::drawJoint(PhysicsJoint& joint)
{
for (auto it = joint._info->getJoints().begin(); it != joint._info->getJoints().end(); ++it)
{
cpConstraint *constraint = *it;
cpBody *body_a = constraint->a;
cpBody *body_b = constraint->b;
const cpConstraintClass *klass = constraint->klass_private;
if(klass == cpPinJointGetClass())
{
cpPinJoint *subJoint = (cpPinJoint *)constraint;
cpVect a = cpvadd(body_a->p, cpvrotate(subJoint->anchr1, body_a->rot));
cpVect b = cpvadd(body_b->p, cpvrotate(subJoint->anchr2, body_b->rot));
_drawNode->drawSegment(PhysicsHelper::cpv2point(a), PhysicsHelper::cpv2point(b), 1, Color4F(0.0f, 0.0f, 1.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(a), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(b), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
}
else if(klass == cpSlideJointGetClass())
{
cpSlideJoint *subJoint = (cpSlideJoint *)constraint;
cpVect a = cpvadd(body_a->p, cpvrotate(subJoint->anchr1, body_a->rot));
cpVect b = cpvadd(body_b->p, cpvrotate(subJoint->anchr2, body_b->rot));
_drawNode->drawSegment(PhysicsHelper::cpv2point(a), PhysicsHelper::cpv2point(b), 1, Color4F(0.0f, 0.0f, 1.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(a), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(b), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
}
else if(klass == cpPivotJointGetClass())
{
cpPivotJoint *subJoint = (cpPivotJoint *)constraint;
cpVect a = cpvadd(body_a->p, cpvrotate(subJoint->anchr1, body_a->rot));
cpVect b = cpvadd(body_b->p, cpvrotate(subJoint->anchr2, body_b->rot));
_drawNode->drawDot(PhysicsHelper::cpv2point(a), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(b), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
}
else if(klass == cpGrooveJointGetClass())
{
cpGrooveJoint *subJoint = (cpGrooveJoint *)constraint;
cpVect a = cpvadd(body_a->p, cpvrotate(subJoint->grv_a, body_a->rot));
cpVect b = cpvadd(body_a->p, cpvrotate(subJoint->grv_b, body_a->rot));
cpVect c = cpvadd(body_b->p, cpvrotate(subJoint->anchr2, body_b->rot));
_drawNode->drawSegment(PhysicsHelper::cpv2point(a), PhysicsHelper::cpv2point(b), 1, Color4F(0.0f, 0.0f, 1.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(c), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
}
else if(klass == cpDampedSpringGetClass())
{
cpDampedSpring *subJoint = (cpDampedSpring *)constraint;
cpVect a = cpvadd(body_a->p, cpvrotate(subJoint->anchr1, body_a->rot));
cpVect b = cpvadd(body_b->p, cpvrotate(subJoint->anchr2, body_b->rot));
_drawNode->drawSegment(PhysicsHelper::cpv2point(a), PhysicsHelper::cpv2point(b), 1, Color4F(0.0f, 0.0f, 1.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(a), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
_drawNode->drawDot(PhysicsHelper::cpv2point(b), 2, Color4F(0.0f, 1.0f, 0.0f, 1.0f));
}
}
}
// This one is complicated and gross. Just don't go there...
// TODO: Comment me!
static int
seg2poly(cpShape *shape1, cpShape *shape2, cpContact **arr)
{
cpSegmentShape *seg = (cpSegmentShape *)shape1;
cpPolyShape *poly = (cpPolyShape *)shape2;
cpPolyShapeAxis *axes = poly->tAxes;
cpFloat segD = cpvdot(seg->tn, seg->ta);
cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r;
cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg->tn), -segD) - seg->r;
if(minNeg > 0.0f || minNorm > 0.0f) return 0;
int mini = 0;
cpFloat poly_min = segValueOnAxis(seg, axes->n, axes->d);
if(poly_min > 0.0f) return 0;
for(int i=0; i<poly->numVerts; i++){
cpFloat dist = segValueOnAxis(seg, axes[i].n, axes[i].d);
if(dist > 0.0f){
return 0;
} else if(dist > poly_min){
poly_min = dist;
mini = i;
}
}
int max = 0;
int num = 0;
cpVect poly_n = cpvneg(axes[mini].n);
cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r));
cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r));
if(cpPolyShapeContainsVert(poly, va))
cpContactInit(addContactPoint(arr, &max, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 0));
if(cpPolyShapeContainsVert(poly, vb))
cpContactInit(addContactPoint(arr, &max, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 1));
// Floating point precision problems here.
// This will have to do for now.
poly_min -= cp_collision_slop;
if(minNorm >= poly_min || minNeg >= poly_min) {
if(minNorm > minNeg)
findPointsBehindSeg(arr, &max, &num, seg, poly, minNorm, 1.0f);
else
findPointsBehindSeg(arr, &max, &num, seg, poly, minNeg, -1.0f);
}
// If no other collision points are found, try colliding endpoints.
if(num == 0){
cpVect poly_a = poly->tVerts[mini];
cpVect poly_b = poly->tVerts[(mini + 1)%poly->numVerts];
if(circle2circleQuery(seg->ta, poly_a, seg->r, 0.0f, arr))
return 1;
if(circle2circleQuery(seg->tb, poly_a, seg->r, 0.0f, arr))
return 1;
if(circle2circleQuery(seg->ta, poly_b, seg->r, 0.0f, arr))
return 1;
if(circle2circleQuery(seg->tb, poly_b, seg->r, 0.0f, arr))
return 1;
}
return num;
}
cpVect
cpArbiterGetPointB(const cpArbiter *arb, int i)
{
cpAssertHard(0 <= i && i < cpArbiterGetCount(arb), "Index error: The specified contact index is invalid for this arbiter");
return cpvadd(arb->body_b->p, arb->contacts[i].r2);
}