static int
begin(cpArbiter *arb, cpSpace *space, void *ignore)
{
CP_ARBITER_GET_SHAPES(arb, a, b);
PlayerStruct *player = a->data;
cpVect n = cpvneg(cpArbiterGetNormal(arb, 0));
if(n.y > 0.0f){
cpArrayPush(player->groundShapes, b);
}
return 1;
}
cpVect
cpArbiterTotalImpulse(const cpArbiter *arb)
{
cpContact *contacts = arb->contacts;
cpVect sum = cpvzero;
for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
cpContact *con = &contacts[i];
sum = cpvadd(sum, cpvmult(con->n, con->jnAcc));
}
return (arb->swappedColl ? sum : cpvneg(sum));
}
void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
{
cpShape *shapea = arb->a;
cpShape *shapeb = arb->b;
arb->e = shapea->e * shapeb->e;
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];
// Calculate the offsets.
con->r1 = cpvsub(con->p, a->p);
con->r2 = cpvsub(con->p, b->p);
// Calculate the mass normal.
cpFloat mass_sum = a->m_inv + b->m_inv;
cpFloat r1cn = cpvcross(con->r1, con->n);
cpFloat r2cn = cpvcross(con->r2, con->n);
cpFloat kn = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn;
con->nMass = 1.0f/kn;
// Calculate the mass tangent.
cpVect t = cpvperp(con->n);
cpFloat r1ct = cpvcross(con->r1, t);
cpFloat r2ct = cpvcross(con->r2, t);
cpFloat kt = mass_sum + a->i_inv*r1ct*r1ct + b->i_inv*r2ct*r2ct;
con->tMass = 1.0f/kt;
// Calculate the target bias velocity.
con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop);
con->jBias = 0.0f;
// Calculate the target bounce velocity.
cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(con->r1), a->w));
cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(con->r2), b->w));
con->bounce = cpvdot(con->n, cpvsub(v2, v1))*arb->e;
// Apply the previous accumulated impulse.
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
cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
cpSegmentShape *seg = (cpSegmentShape *)shape;
cpVect n = seg->tn;
// flip n if a is behind the axis
if(cpvdot(a, n) < cpvdot(seg->ta, n))
n = cpvneg(n);
cpFloat an = cpvdot(a, n);
cpFloat bn = cpvdot(b, n);
cpFloat d = cpvdot(seg->ta, n) + seg->r;
cpFloat t = (d - an)/(bn - an);
if(0.0f < t && t < 1.0f) {
cpVect point = cpvlerp(a, b, t);
cpFloat dt = -cpvcross(seg->tn, point);
cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
if(dtMin < dt && dt < dtMax) {
info->shape = shape;
info->t = t;
info->n = n;
return; // don't continue on and check endcaps
}
}
if(seg->r) {
cpSegmentQueryInfo info1;
info1.shape = NULL;
cpSegmentQueryInfo info2;
info2.shape = NULL;
circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1);
circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2);
if(info1.shape && !info2.shape) {
(*info) = info1;
} else if(info2.shape && !info1.shape) {
(*info) = info2;
} else if(info1.shape && info2.shape) {
if(info1.t < info2.t) {
(*info) = info1;
} else {
(*info) = info2;
}
}
}
}
// Recursive implementatino of the GJK loop.
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS) {
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
// TODO: should this be an area2x check?
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f) {
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
) {
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
return EPA(ctx, v0, p, v1);
} else {
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))) {
// The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
// p was closer to the origin than our existing edge.
// Need to figure out which existing point to drop.
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)) {
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
static struct cpShapeMassInfo
cpPolyShapeMassInfo(cpFloat mass, int count, const cpVect *verts, cpFloat radius)
{
// TODO moment is approximate due to radius.
cpVect centroid = cpCentroidForPoly(count, verts);
struct cpShapeMassInfo info = {
mass, cpMomentForPoly(1.0f, count, verts, cpvneg(centroid), radius),
centroid,
cpAreaForPoly(count, verts, radius),
};
return info;
}
// Collide circles to segment shapes.
static int
circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact **con)
{
cpCircleShape *circ = (cpCircleShape *)circleShape;
cpSegmentShape *seg = (cpSegmentShape *)segmentShape;
// Radius sum
cpFloat rsum = circ->r + seg->r;
// Calculate normal distance from segment.
cpFloat dn = cpvdot(seg->tn, circ->tc) - cpvdot(seg->ta, seg->tn);
cpFloat dist = cpfabs(dn) - rsum;
if(dist > 0.0f) return 0;
// Calculate tangential distance along segment.
cpFloat dt = -cpvcross(seg->tn, circ->tc);
cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
// Decision tree to decide which feature of the segment to collide with.
if(dt < dtMin){
if(dt < (dtMin - rsum)){
return 0;
} else {
return circle2circleQuery(circ->tc, seg->ta, circ->r, seg->r, con);
}
} else {
if(dt < dtMax){
cpVect n = (dn < 0.0f) ? seg->tn : cpvneg(seg->tn);
(*con) = (cpContact *)cpmalloc(sizeof(cpContact));
cpContactInit(
(*con),
cpvadd(circ->tc, cpvmult(n, circ->r + dist*0.5f)),
n,
dist,
0
);
return 1;
} else {
if(dt < (dtMax + rsum)) {
return circle2circleQuery(circ->tc, seg->tb, circ->r, seg->r, con);
} else {
return 0;
}
}
}
return 1;
}
cpVect
cpArbiterTotalImpulse(const cpArbiter *arb)
{
struct cpContact *contacts = arb->contacts;
cpVect n = arb->n;
cpVect sum = cpvzero;
for(int i=0, count=cpArbiterGetCount(arb); i<count; i++){
struct cpContact *con = &contacts[i];
sum = cpvadd(sum, cpvrotate(n, cpv(con->jnAcc, con->jtAcc)));
}
return (arb->swapped ? sum : cpvneg(sum));
return cpvzero;
}
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f){
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration + 1);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
return EPA(ctx, v0, p, v1);
} else {
// The new point must be farther along the normal than the existing points.
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
void
cpArbiterSetContactPointSet(cpArbiter *arb, cpContactPointSet *set)
{
int count = set->count;
cpAssertHard(count == arb->count, "The number of contact points cannot be changed.");
cpBool swapped = arb->swapped;
arb->n = (swapped ? cpvneg(set->normal) : set->normal);
for(int i=0; i<count; i++){
// Convert back to CoG relative offsets.
cpVect p1 = set->points[i].pointA;
cpVect p2 = set->points[i].pointB;
arb->contacts[i].r1 = cpvsub(swapped ? p2 : p1, arb->body_a->p);
arb->contacts[i].r2 = cpvsub(swapped ? p1 : p2, arb->body_b->p);
}
}
void Slice::SliceShapePostStep(cpSpace *space, cpShape *shape, struct SliceContext *context)
{
cpVect a = context->a;
cpVect b = context->b;
// Clipping plane normal and distance.
cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
cpFloat dist = cpvdot(a, n);
ClipPoly(space, shape, n, dist);
ClipPoly(space, shape, cpvneg(n), -dist);
cpBody *body = cpShapeGetBody(shape);
cpSpaceRemoveShape(space, shape);
cpSpaceRemoveBody(space, body);
cpShapeFree(shape);
cpBodyFree(body);
}
static int
preSolve(cpArbiter *arb, cpSpace *space, void *ignore)
{
CP_ARBITER_GET_SHAPES(arb, a, b);
PlayerStruct *player = a->data;
if(arb->stamp > 0){
a->u = player->u;
// pick the most upright jump normal each frame
cpVect n = cpvneg(cpArbiterGetNormal(arb, 0));
if(n.y >= player->groundNormal.y){
player->groundNormal = n;
}
}
return 1;
}
static void
ClipPoly(cpSpace *space, cpShape *shape, cpVect n, cpFloat dist)
{
cpBody *body = cpShapeGetBody(shape);
int count = cpPolyShapeGetNumVerts(shape);
int clippedCount = 0;
cpVect *clipped = (cpVect *)alloca((count + 1)*sizeof(cpVect));
for(int i=0, j=count-1; i<count; j=i, i++){
cpVect a = cpBodyLocal2World(body, cpPolyShapeGetVert(shape, j));
cpFloat a_dist = cpvdot(a, n) - dist;
if(a_dist < 0.0){
clipped[clippedCount] = a;
clippedCount++;
}
cpVect b = cpBodyLocal2World(body, cpPolyShapeGetVert(shape, i));
cpFloat b_dist = cpvdot(b, n) - dist;
if(a_dist*b_dist < 0.0f){
cpFloat t = cpfabs(a_dist)/(cpfabs(a_dist) + cpfabs(b_dist));
clipped[clippedCount] = cpvlerp(a, b, t);
clippedCount++;
}
}
cpVect centroid = cpCentroidForPoly(clippedCount, clipped);
cpFloat mass = cpAreaForPoly(clippedCount, clipped)*DENSITY;
cpFloat moment = cpMomentForPoly(mass, clippedCount, clipped, cpvneg(centroid));
cpBody *new_body = cpSpaceAddBody(space, cpBodyNew(mass, moment));
cpBodySetPos(new_body, centroid);
cpBodySetVel(new_body, cpBodyGetVelAtWorldPoint(body, centroid));
cpBodySetAngVel(new_body, cpBodyGetAngVel(body));
cpShape *new_shape = cpSpaceAddShape(space, cpPolyShapeNew(new_body, clippedCount, clipped, cpvneg(centroid)));
// Copy whatever properties you have set on the original shape that are important
cpShapeSetFriction(new_shape, cpShapeGetFriction(shape));
}
// Add contacts for probably penetrating vertexes.
// This handles the degenerate case where an overlap was detected, but no vertexes fall inside
// the opposing polygon. (like a star of david)
static inline int
findVertsFallback(cpContact *arr, const cpPolyShape *poly1, const cpPolyShape *poly2, const cpVect n, const cpFloat dist)
{
int num = 0;
for(int i=0; i<poly1->numVerts; i++){
cpVect v = poly1->tVerts[i];
if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n)))
cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i));
}
for(int i=0; i<poly2->numVerts; i++){
cpVect v = poly2->tVerts[i];
if(cpPolyShapeContainsVertPartial(poly1, v, n))
cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i));
}
return num;
}
// This one is less gross, but still gross.
// TODO: Comment me!
static int
circle2poly(cpShape *shape1, cpShape *shape2, cpContact **con)
{
cpCircleShape *circ = (cpCircleShape *)shape1;
cpPolyShape *poly = (cpPolyShape *)shape2;
cpPolyShapeAxis *axes = poly->tAxes;
int mini = 0;
cpFloat min = cpvdot(axes->n, circ->tc) - axes->d - circ->r;
for(int i=0; i<poly->numVerts; i++){
cpFloat dist = cpvdot(axes[i].n, circ->tc) - axes[i].d - circ->r;
if(dist > 0.0f){
return 0;
} else if(dist > min) {
min = dist;
mini = i;
}
}
cpVect n = axes[mini].n;
cpVect a = poly->tVerts[mini];
cpVect b = poly->tVerts[(mini + 1)%poly->numVerts];
cpFloat dta = cpvcross(n, a);
cpFloat dtb = cpvcross(n, b);
cpFloat dt = cpvcross(n, circ->tc);
if(dt < dtb){
return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con);
} else if(dt < dta) {
(*con) = (cpContact *)cpmalloc(sizeof(cpContact));
cpContactInit(
(*con),
cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)),
cpvneg(n),
min,
0
);
return 1;
} else {
return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con);
}
}
// This one is less gross, but still gross.
// TODO: Comment me!
static int
circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con)
{
cpCircleShape *circ = (cpCircleShape *)shape1;
cpPolyShape *poly = (cpPolyShape *)shape2;
cpSplittingPlane *planes = poly->tPlanes;
int mini = 0;
cpFloat min = cpSplittingPlaneCompare(planes[0], circ->tc) - circ->r;
for(int i=0; i<poly->numVerts; i++){
cpFloat dist = cpSplittingPlaneCompare(planes[i], circ->tc) - circ->r;
if(dist > 0.0f){
return 0;
} else if(dist > min) {
min = dist;
mini = i;
}
}
cpVect n = planes[mini].n;
cpVect a = poly->tVerts[mini];
cpVect b = poly->tVerts[(mini + 1)%poly->numVerts];
cpFloat dta = cpvcross(n, a);
cpFloat dtb = cpvcross(n, b);
cpFloat dt = cpvcross(n, circ->tc);
if(dt < dtb){
return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con);
} else if(dt < dta) {
cpContactInit(
con,
cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)),
cpvneg(n),
min,
0
);
return 1;
} else {
return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con);
}
}
// Collide poly shapes together.
static int
poly2poly(cpShape *shape1, cpShape *shape2, cpContact **arr)
{
cpPolyShape *poly1 = (cpPolyShape *)shape1;
cpPolyShape *poly2 = (cpPolyShape *)shape2;
cpFloat min1;
int mini1 = findMSA(poly2, poly1->tAxes, poly1->numVerts, &min1);
if(mini1 == -1) return 0;
cpFloat min2;
int mini2 = findMSA(poly1, poly2->tAxes, poly2->numVerts, &min2);
if(mini2 == -1) return 0;
// There is overlap, find the penetrating verts
if(min1 > min2)
return findVerts(arr, poly1, poly2, poly1->tAxes[mini1].n, min1);
else
return findVerts(arr, poly1, poly2, cpvneg(poly2->tAxes[mini2].n), min2);
}
static void
cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
{
cpVect n = seg->tn;
cpFloat d = cpvdot(cpvsub(seg->ta, a), n);
cpFloat r = seg->r + r2;
cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n);
cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a);
// Make the endpoints relative to 'a' and move them by the thickness of the segment.
cpVect seg_a = cpvadd(seg->ta, seg_offset);
cpVect seg_b = cpvadd(seg->tb, seg_offset);
cpVect delta = cpvsub(b, a);
if(cpvcross(delta, seg_a)*cpvcross(delta, seg_b) <= 0.0f){
cpFloat d_offset = d + (d > 0.0f ? -r : r);
cpFloat ad = -d_offset;
cpFloat bd = cpvdot(delta, n) - d_offset;
if(ad*bd < 0.0f){
cpFloat t = ad/(ad - bd);
info->shape = (cpShape *)seg;
info->point = cpvsub(cpvlerp(a, b, t), cpvmult(flipped_n, r2));
info->normal = flipped_n;
info->alpha = t;
}
} else if(r != 0.0f){
cpSegmentQueryInfo info1 = {NULL, b, cpvzero, 1.0f};
cpSegmentQueryInfo info2 = {NULL, b, cpvzero, 1.0f};
CircleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, r2, &info1);
CircleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, r2, &info2);
if(info1.alpha < info2.alpha){
(*info) = info1;
} else {
(*info) = info2;
}
}
}
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);
}
}
// Callback from the spatial hash.
static cpCollisionID
ShapeQuery(cpShape *a, cpShape *b, cpCollisionID id, struct ShapeQueryContext *context)
{
// Reject any of the simple cases
if(
(a->group && a->group == b->group) ||
!(a->layers & b->layers) ||
a == b
) return id;
cpContact contacts[CP_MAX_CONTACTS_PER_ARBITER];
int numContacts = 0;
// Shape 'a' should have the lower shape type. (required by cpCollideShapes() )
if(a->klass->type <= b->klass->type){
numContacts = cpCollideShapes(a, b, &id, contacts);
} else {
numContacts = cpCollideShapes(b, a, &id, contacts);
for(int i=0; i<numContacts; i++) contacts[i].n = cpvneg(contacts[i].n);
}
if(numContacts){
context->anyCollision = !(a->sensor || b->sensor);
if(context->func){
cpContactPointSet set;
set.count = numContacts;
for(int i=0; i<set.count; i++){
set.points[i].point = contacts[i].p;
set.points[i].normal = contacts[i].n;
set.points[i].dist = contacts[i].dist;
}
context->func(b, &set, context->data);
}
}
return id;
}
// Add contacts for penetrating vertexes.
static inline int
findVerts(cpContact *arr, cpPolyShape *poly1, cpPolyShape *poly2, cpVect n, cpFloat dist)
{
int num = 0;
for(int i=0; i<poly1->numVerts; i++){
cpVect v = poly1->tVerts[i];
if(cpPolyShapeContainsVertPartial(poly2, v, cpvneg(n)))
cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1->shape.hashid, i));
}
for(int i=0; i<poly2->numVerts; i++){
cpVect v = poly2->tVerts[i];
if(cpPolyShapeContainsVertPartial(poly1, v, n))
cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2->shape.hashid, i));
}
// if(!num)
// addContactPoint(arr, &size, &num, cpContactNew(shape1->body->p, n, dist, 0));
return num;
}
cpContactPointSet
cpArbiterGetContactPointSet(const cpArbiter *arb)
{
cpContactPointSet set;
set.count = cpArbiterGetCount(arb);
cpBool swapped = arb->swapped;
cpVect n = arb->n;
set.normal = (swapped ? cpvneg(n) : n);
for(int i=0; i<set.count; i++){
// Contact points are relative to body CoGs;
cpVect p1 = cpvadd(arb->body_a->p, arb->contacts[i].r1);
cpVect p2 = cpvadd(arb->body_b->p, arb->contacts[i].r2);
set.points[i].pointA = (swapped ? p2 : p1);
set.points[i].pointB = (swapped ? p1 : p2);
set.points[i].distance = cpvdot(cpvsub(p2, p1), n);
}
return set;
}
static struct Edge
SupportEdgeForSegment(const cpSegmentShape *seg, const cpVect n)
{
cpHashValue hashid = seg->shape.hashid;
if(cpvdot(seg->tn, n) > 0.0) {
struct Edge edge = {{seg->ta, CP_HASH_PAIR(hashid, 0)}, {seg->tb, CP_HASH_PAIR(hashid, 1)}, seg->r, seg->tn};
return edge;
} else {
struct Edge edge = {{seg->tb, CP_HASH_PAIR(hashid, 1)}, {seg->ta, CP_HASH_PAIR(hashid, 0)}, seg->r, cpvneg(seg->tn)};
return edge;
}
}
cpVect *hullVerts = alloca(mdiffCount*sizeof(cpVect));
int hullCount = cpConvexHull(mdiffCount, mdiffVerts, hullVerts, NULL, 0.0);
ChipmunkDebugDrawPolygon(hullCount, hullVerts, 0.0, RGBAColor(1, 0, 0, 1), RGBAColor(1, 0, 0, 0.25));
#endif
struct MinkowskiPoint v0, v1;
if(*id) {
// Use the minkowski points from the last frame as a starting point using the cached indexes.
v0 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>>24)&0xFF), ShapePoint(ctx->shape2, (*id>>16)&0xFF));
v1 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>> 8)&0xFF), ShapePoint(ctx->shape2, (*id )&0xFF));
} else {
// No cached indexes, use the shapes' bounding box centers as a guess for a starting axis.
cpVect axis = cpvperp(cpvsub(cpBBCenter(ctx->shape1->bb), cpBBCenter(ctx->shape2->bb)));
v0 = Support(ctx, axis);
v1 = Support(ctx, cpvneg(axis));
}
struct ClosestPoints points = GJKRecurse(ctx, v0, v1, 1);
*id = points.id;
return points;
}
//MARK: Contact Clipping
// Given two support edges, find contact point pairs on their surfaces.
static inline void
ContactPoints(const struct Edge e1, const struct Edge e2, const struct ClosestPoints points, struct cpCollisionInfo *info)
{
cpFloat mindist = e1.r + e2.r;
if(points.d <= mindist) {
// apply a lasting external force to the center of gravity.
// also apply lasting forces perpendicluar to vectors from the c.o.g.
// (in other words, push and rotate the object)
// these need to be subtracted later to stop their effect
void applyforces(struct objnode *player, struct forces f)
{
cpBodyApplyForce(player->b, f.force, cpvzero);
cpBodyApplyForce(player->b, f.tforce, cpv(RLEN, 0));
cpBodyApplyForce(player->b, cpvneg(f.tforce), cpv(-RLEN, 0));
}
cpVect * bmx_cpvect_negate(cpVect * vec) {
return bmx_cpvect_new(cpvneg(*vec));
}
cpBool Buoyancy::WaterPreSolve(cpArbiter *arb, cpSpace *space, void *ptr)
{
CP_ARBITER_GET_SHAPES(arb, water, poly);
cpBody *body = cpShapeGetBody(poly);
// Get the top of the water sensor bounding box to use as the water level.
cpFloat level = cpShapeGetBB(water).t;
// Clip the polygon against the water level
int count = cpPolyShapeGetCount(poly);
int clippedCount = 0;
#ifdef _MSC_VER
// MSVC is pretty much the only compiler in existence that doesn't support variable sized arrays.
cpVect clipped[10];
#else
cpVect clipped[count + 1];
#endif
for(int i=0, j=count-1; i<count; j=i, i++){
cpVect a = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, j));
cpVect b = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, i));
if(a.y < level){
clipped[clippedCount] = a;
clippedCount++;
}
cpFloat a_level = a.y - level;
cpFloat b_level = b.y - level;
if(a_level*b_level < 0.0f){
cpFloat t = cpfabs(a_level)/(cpfabs(a_level) + cpfabs(b_level));
clipped[clippedCount] = cpvlerp(a, b, t);
clippedCount++;
}
}
// Calculate buoyancy from the clipped polygon area
cpFloat clippedArea = cpAreaForPoly(clippedCount, clipped, 0.0f);
cpFloat displacedMass = clippedArea*FLUID_DENSITY;
cpVect centroid = cpCentroidForPoly(clippedCount, clipped);
cpDataPointer data = ptr;
DrawPolygon(clippedCount, clipped, 0.0f, RGBAColor(0, 0, 1, 1), RGBAColor(0, 0, 1, 0.1f), data);
DrawDot(5, centroid, RGBAColor(0, 0, 1, 1), data);
cpFloat dt = cpSpaceGetCurrentTimeStep(space);
cpVect g = cpSpaceGetGravity(space);
// Apply the buoyancy force as an impulse.
cpBodyApplyImpulseAtWorldPoint(body, cpvmult(g, -displacedMass*dt), centroid);
// Apply linear damping for the fluid drag.
cpVect v_centroid = cpBodyGetVelocityAtWorldPoint(body, centroid);
cpFloat k = k_scalar_body(body, centroid, cpvnormalize(v_centroid));
cpFloat damping = clippedArea*FLUID_DRAG*FLUID_DENSITY;
cpFloat v_coef = cpfexp(-damping*dt*k); // linear drag
// cpFloat v_coef = 1.0/(1.0 + damping*dt*cpvlength(v_centroid)*k); // quadratic drag
cpBodyApplyImpulseAtWorldPoint(body, cpvmult(cpvsub(cpvmult(v_centroid, v_coef), v_centroid), 1.0/k), centroid);
// Apply angular damping for the fluid drag.
cpVect cog = cpBodyLocalToWorld(body, cpBodyGetCenterOfGravity(body));
cpFloat w_damping = cpMomentForPoly(FLUID_DRAG*FLUID_DENSITY*clippedArea, clippedCount, clipped, cpvneg(cog), 0.0f);
cpBodySetAngularVelocity(body, cpBodyGetAngularVelocity(body)*cpfexp(-w_damping*dt/cpBodyGetMoment(body)));
return cpTrue;
}
// 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;
}
