static void
cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
cpSplittingPlane *axes = poly->tPlanes;
cpVect *verts = poly->tVerts;
int numVerts = poly->numVerts;
for(int i=0; i<numVerts; i++){
cpVect n = axes[i].n;
cpFloat an = cpvdot(a, n);
if(axes[i].d > an) continue;
cpFloat bn = cpvdot(b, n);
cpFloat t = (axes[i].d - an)/(bn - an);
if(t < 0.0f || 1.0f < t) continue;
cpVect point = cpvlerp(a, b, t);
cpFloat dt = -cpvcross(n, point);
cpFloat dtMin = -cpvcross(n, verts[i]);
cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]);
if(dtMin <= dt && dt <= dtMax){
info->shape = (cpShape *)poly;
info->t = t;
info->n = n;
}
}
}
static void
preStep(cpPulleyJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody* b1 = joint->constraint.a;
cpBody* b2 = joint->constraint.b;
joint->r1 = cpvrotate(joint->anchr1, b1->rot);
joint->r2 = cpvrotate(joint->anchr2, b2->rot);
cpVect p1 = cpvadd(b1->p, joint->r1);
cpVect p2 = cpvadd(b2->p, joint->r2);
//Catto claimed that these needed to be "grounded" pts
cpVect s1 = cpBodyLocal2World(joint->c, joint->anchr3a);
cpVect s2 = cpBodyLocal2World(joint->c, joint->anchr3b);
// Get the pulley axes.
joint->u1 = cpvsub(p1, s1);
joint->u2 = cpvsub(p2, s2);
// Lengths
cpFloat length1 = cpvlength(joint->u1);
cpFloat length2 = cpvlength(joint->u2);
// Check constraints
joint->u1 = (length1 > .01) ? cpvmult(joint->u1, 1.0f/length1) : cpvzero;
joint->u2 = (length2 > .01) ? cpvmult(joint->u2, 1.0f/length2) : cpvzero;
// Compute 'C'
cpFloat C = joint->constant - length1 - joint->ratio * length2;
// Set state based on lengths
joint->state = (C > 0.0f) ? 0 : 1;
joint->limitState1 = (length1 < joint->max1) ? 0 : 1;
joint->limitState2 = (length2 < joint->max2) ? 0 : 1;
// Compute effective mass.
cpFloat cr1u1 = cpvcross(joint->r1, joint->u1);
cpFloat cr2u2 = cpvcross(joint->r2, joint->u2);
// Set Mass Limits
joint->limitMass1 = b1->m_inv + b1->i_inv * cr1u1 * cr1u1;
joint->limitMass2 = b2->m_inv + b2->i_inv * cr2u2 * cr2u2;
joint->pulleyMass = joint->limitMass1 + joint->ratio * joint->ratio * joint->limitMass2;
// Check against evil
//cpAssert(joint->limitMass1 != 0.0f, "Calculated Pulley Limit(1) is Zero");
//cpAssert(joint->limitMass2 != 0.0f, "Calculated Pulley Limit(2) is Zero");
//cpAssert(joint->pulleyMass != 0.0f, "Calculated Pulley Mass is Zero");
// We want the inverse's
joint->limitMass1 = 1.0f / joint->limitMass1;
joint->limitMass2 = 1.0f / joint->limitMass2;
joint->pulleyMass = 1.0f / joint->pulleyMass;
// Reset accumulations, could also warm start here
joint->jnAcc = 0.0f;
joint->jnAccLim1 = 0.0f;
joint->jnAccLim2 = 0.0f;
}
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.
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);
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
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){
int count2 = 1;
struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
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 {
cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
}
// 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 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;
}
}
}
}
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);
}
}
// 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;
}
static int
QHullPartition(cpVect *verts, int count, cpVect a, cpVect b, cpFloat tol)
{
if(count == 0) return 0;
cpFloat max = 0;
int pivot = 0;
cpVect delta = cpvsub(b, a);
cpFloat valueTol = tol*cpvlength(delta);
int head = 0;
for(int tail = count-1; head <= tail;){
cpFloat value = cpvcross(delta, cpvsub(verts[head], a));
if(value > valueTol){
if(value > max){
max = value;
pivot = head;
}
head++;
} else {
SWAP(verts[head], verts[tail]);
tail--;
}
}
// move the new pivot to the front if it's not already there.
if(pivot != 0) SWAP(verts[0], verts[pivot]);
return head;
}
void
cpBodyApplyForce(cpBody *body, cpVect force, cpVect r)
{
cpBodyActivate(body);
body->f = cpvadd(body->f, force);
body->t += cpvcross(r, force);
}
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);
}
}
}
}
}
// Identify vertexes that have penetrated the segment.
static inline void
findPointsBehindSeg(cpContact **arr, int *max, int *num, cpSegmentShape *seg, cpPolyShape *poly, cpFloat pDist, cpFloat coef)
{
cpFloat dta = cpvcross(seg->tn, seg->ta);
cpFloat dtb = cpvcross(seg->tn, seg->tb);
cpVect n = cpvmult(seg->tn, coef);
for(int i=0; i<poly->numVerts; i++){
cpVect v = poly->tVerts[i];
if(cpvdot(v, n) < cpvdot(seg->tn, seg->ta)*coef + seg->r){
cpFloat dt = cpvcross(seg->tn, v);
if(dta >= dt && dt >= dtb){
cpContactInit(addContactPoint(arr, max, num), v, n, pDist, CP_HASH_PAIR(poly->shape.hashid, i));
}
}
}
}
static void
preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
// calculate endpoints in worldspace
cpVect ta = cpBodyLocal2World(a, joint->grv_a);
cpVect tb = cpBodyLocal2World(a, joint->grv_b);
// calculate axis
cpVect n = cpvrotate(joint->grv_n, a->rot);
cpFloat d = cpvdot(ta, n);
joint->grv_tn = n;
joint->r2 = cpvrotate(joint->anchr2, b->rot);
// calculate tangential distance along the axis of r2
cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
// calculate clamping factor and r2
if(td <= cpvcross(ta, n)){
joint->clamp = 1.0f;
joint->r1 = cpvsub(ta, a->p);
} else if(td >= cpvcross(tb, n)){
joint->clamp = -1.0f;
joint->r1 = cpvsub(tb, a->p);
} else {
joint->clamp = 0.0f;
joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
}
// Calculate mass tensor
k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2);
// compute max impulse
joint->jMaxLen = J_MAX(joint, dt);
// calculate bias velocity
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
joint->bias = cpvclamp(cpvmult(delta, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
// apply accumulated impulse
apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc);
}
cpFloat
cpAreaForPoly(const int numVerts, const cpVect *verts)
{
cpFloat area = 0.0f;
for(int i=0; i<numVerts; i++){
area += cpvcross(verts[i], verts[(i+1)%numVerts]);
}
return area/2.0f;
}
// 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);
}
}
cpFloat
cpAreaForPoly(const int count, const cpVect *verts, cpFloat radius)
{
cpFloat area = 0.0f;
for(int i=0; i<count; i++){
area += cpvcross(verts[(i+1)%count], verts[i]);
}
// TODO add circle area + perimeter*radius
return -area/2.0f;
}
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;
}
}
}
static void
cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
{
struct cpSplittingPlane *planes = poly->planes;
int count = poly->count;
cpFloat r = poly->r;
cpFloat rsum = r + r2;
for(int i=0; i<count; i++){
cpVect n = planes[i].n;
cpFloat an = cpvdot(a, n);
cpFloat d = an - cpvdot(planes[i].v0, n) - rsum;
if(d < 0.0f) continue;
cpFloat bn = cpvdot(b, n);
cpFloat t = d/(an - bn);
if(t < 0.0f || 1.0f < t) continue;
cpVect point = cpvlerp(a, b, t);
cpFloat dt = cpvcross(n, point);
cpFloat dtMin = cpvcross(n, planes[(i - 1 + count)%count].v0);
cpFloat dtMax = cpvcross(n, planes[i].v0);
if(dtMin <= dt && dt <= dtMax){
info->shape = (cpShape *)poly;
info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2));
info->normal = n;
info->alpha = t;
}
}
// Also check against the beveled vertexes.
if(rsum > 0.0f){
for(int i=0; i<count; i++){
cpSegmentQueryInfo circle_info = {NULL, b, cpvzero, 1.0f};
CircleSegmentQuery(&poly->shape, planes[i].v0, r, a, b, r2, &circle_info);
if(circle_info.alpha < info->alpha) (*info) = circle_info;
}
}
}
static void
cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
cpSplittingPlane *axes = poly->tPlanes;
cpVect *verts = poly->tVerts;
int numVerts = poly->numVerts;
cpFloat r = poly->r;
for(int i=0; i<numVerts; i++){
cpVect n = axes[i].n;
cpFloat an = cpvdot(a, n);
cpFloat d = axes[i].d + r - an;
if(d > 0.0f) continue;
cpFloat bn = cpvdot(b, n);
cpFloat t = d/(bn - an);
if(t < 0.0f || 1.0f < t) continue;
cpVect point = cpvlerp(a, b, t);
cpFloat dt = -cpvcross(n, point);
cpFloat dtMin = -cpvcross(n, verts[(i - 1 + numVerts)%numVerts]);
cpFloat dtMax = -cpvcross(n, verts[i]);
if(dtMin <= dt && dt <= dtMax){
info->shape = (cpShape *)poly;
info->t = t;
info->n = n;
}
}
// Also check against the beveled vertexes.
if(r > 0.0f){
for(int i=0; i<numVerts; i++){
cpSegmentQueryInfo circle_info = {NULL, 1.0f, cpvzero};
CircleSegmentQuery(&poly->shape, verts[i], r, a, b, &circle_info);
if(circle_info.t < info->t) (*info) = circle_info;
}
}
}
static void
preStep(cpGrooveJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
// calculate endpoints in worldspace
cpVect ta = cpTransformPoint(a->transform, joint->grv_a);
cpVect tb = cpTransformPoint(a->transform, joint->grv_b);
// calculate axis
cpVect n = cpTransformVect(a->transform, joint->grv_n);
cpFloat d = cpvdot(ta, n);
joint->grv_tn = n;
joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
// calculate tangential distance along the axis of r2
cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
// calculate clamping factor and r2
if(td <= cpvcross(ta, n)){
joint->clamp = 1.0f;
joint->r1 = cpvsub(ta, a->p);
} else if(td >= cpvcross(tb, n)){
joint->clamp = -1.0f;
joint->r1 = cpvsub(tb, a->p);
} else {
joint->clamp = 0.0f;
joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
}
// Calculate mass tensor
joint->k = k_tensor(a, b, joint->r1, joint->r2);
// calculate bias velocity
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
}
int
cpPolyValidate(cpVect *verts, int numVerts)
{
for(int i=0; i<numVerts; i++){
cpVect a = verts[i];
cpVect b = verts[(i+1)%numVerts];
cpVect c = verts[(i+2)%numVerts];
if(cpvcross(cpvsub(b, a), cpvsub(c, b)) > 0.0f)
return 0;
}
return 1;
}
cpBool
cpPolyValidate(const cpVect *verts, const int numVerts)
{
for(int i=0; i<numVerts; i++){
cpVect a = verts[i];
cpVect b = verts[(i+1)%numVerts];
cpVect c = verts[(i+2)%numVerts];
if(cpvcross(cpvsub(b, a), cpvsub(c, b)) > 0.0f)
return cpFalse;
}
return cpTrue;
}
static cpBool
cpSegmentShapePointQuery(cpShape *shape, cpVect p){
if(!cpBBcontainsVect(shape->bb, p)) return cpFalse;
cpSegmentShape *seg = (cpSegmentShape *)shape;
// Calculate normal distance from segment.
cpFloat dn = cpvdot(seg->tn, p) - cpvdot(seg->ta, seg->tn);
cpFloat dist = cpfabs(dn) - seg->r;
if(dist > 0.0f) return cpFalse;
// Calculate tangential distance along segment.
cpFloat dt = -cpvcross(seg->tn, p);
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 - seg->r)){
return cpFalse;
} else {
return cpvlengthsq(cpvsub(seg->ta, p)) < (seg->r*seg->r);
}
} else {
if(dt < dtMax){
return cpTrue;
} else {
if(dt < (dtMax + seg->r)) {
return cpvlengthsq(cpvsub(seg->tb, p)) < (seg->r*seg->r);
} else {
return cpFalse;
}
}
}
return cpTrue;
}
cpVect
cpCentroidForPoly(const int numVerts, const cpVect *verts)
{
cpFloat sum = 0.0f;
cpVect vsum = cpvzero;
for(int i=0; i<numVerts; i++){
cpVect v1 = verts[i];
cpVect v2 = verts[(i+1)%numVerts];
cpFloat cross = cpvcross(v1, v2);
sum += cross;
vsum = cpvadd(vsum, cpvmult(cpvadd(v1, v2), cross));
}
return cpvmult(vsum, 1.0f/(3.0f*sum));
}
cpFloat
cpMomentForPoly(cpFloat m, const int numVerts, const cpVect *verts, cpVect offset)
{
cpFloat sum1 = 0.0f;
cpFloat sum2 = 0.0f;
for(int i=0; i<numVerts; i++){
cpVect v1 = cpvadd(verts[i], offset);
cpVect v2 = cpvadd(verts[(i+1)%numVerts], offset);
cpFloat a = cpvcross(v2, v1);
cpFloat b = cpvdot(v1, v1) + cpvdot(v1, v2) + cpvdot(v2, v2);
sum1 += a*b;
sum2 += a;
}
return (m*sum1)/(6.0f*sum2);
}
cpFloat
cpMomentForPoly(cpFloat m, const int count, const cpVect *verts, cpVect offset, cpFloat radius)
{
if(count == 2) return cpMomentForSegment(m, verts[0], verts[1], radius);
cpFloat sum1 = 0.0f;
cpFloat sum2 = 0.0f;
for(int i=0; i<count; i++){
cpVect v1 = cpvadd(verts[i], offset);
cpVect v2 = cpvadd(verts[(i+1)%count], offset);
cpFloat a = cpvcross(v2, v1);
cpFloat b = cpvdot(v1, v1) + cpvdot(v1, v2) + cpvdot(v2, v2);
sum1 += a*b;
sum2 += a;
}
// TODO account for radius.
return (m*sum1)/(6.0f*sum2);
}
void
cpBodyApplyForce(cpBody *body, cpVect f, cpVect r)
{
body->f = cpvadd(body->f, f);
body->t += cpvcross(r, f);
}
static inline cpVect
grooveConstrain(cpGrooveJoint *joint, cpVect j, cpFloat dt){
cpVect n = joint->grv_tn;
cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n);
return cpvclamp(jClamp, joint->constraint.maxForce*dt);
}
static inline cpVect
grooveConstrain(cpGrooveJoint *joint, cpVect j){
cpVect n = joint->grv_tn;
cpVect jClamp = (joint->clamp*cpvcross(j, n) > 0.0f) ? j : cpvproject(j, n);
return cpvclamp(jClamp, joint->jMaxLen);
}
cpFloat bmx_cpvect_cross(cpVect * vec, cpVect * vec1) {
return cpvcross(*vec, *vec1);
}
