void
cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b)
{
// Arbiters without contact data may exist if a collision function rejected the collision.
if(arb->contacts){
// Iterate over the possible pairs to look for hash value matches.
for(int i=0; i<arb->numContacts; i++){
cpContact *old = &arb->contacts[i];
for(int j=0; j<numContacts; j++){
cpContact *new_contact = &contacts[j];
// This could trigger false positives, but is fairly unlikely nor serious if it does.
if(new_contact->hash == old->hash){
// Copy the persistant contact information.
new_contact->jnAcc = old->jnAcc;
new_contact->jtAcc = old->jtAcc;
}
}
}
}
arb->contacts = contacts;
arb->numContacts = numContacts;
arb->handler = handler;
arb->swappedColl = (a->collision_type != handler->a);
arb->e = a->e * b->e;
arb->u = a->u * b->u;
arb->surface_vr = cpvsub(a->surface_v, b->surface_v);
// For collisions between two similar primitive types, the order could have been swapped.
arb->a = a; arb->body_a = a->body;
arb->b = b; arb->body_b = b->body;
// mark it as new if it's been cached
if(arb->state == cpArbiterStateCached) arb->state = cpArbiterStateFirstColl;
}
/* Mouse handling is a bit tricky. We want the user to move
* tiles using the mouse but because tiles are dynamic bodies
* managed by Chipmunk2D, we cannot directly control them.
* This is resolved by creating a pivot joint between an
* invisible mouse body that we can control and the tile body
* that we cannot directly control.
*/
static void apply_mouse_motion(struct state* state)
{
struct mouse m;
update_mouse(&m);
int w, h;
get_screen_size(&w, &h);
int x = m.x_position * w;
int y = m.y_position * h;
cpVect mouse_pos = cpv(x, y);
cpVect new_point =
cpvlerp(cpBodyGetPosition(state->mouse_body), mouse_pos, 0.25f);
cpBodySetVelocity(
state->mouse_body,
cpvmult(cpvsub(new_point, cpBodyGetPosition(state->mouse_body)),
60.0f));
cpBodySetPosition(state->mouse_body, new_point);
if (m.left_click && state->mouse_joint == NULL) {
cpFloat radius = 5.0;
cpPointQueryInfo info = { 0 };
cpShape* shape = cpSpacePointQueryNearest(state->space, mouse_pos,
radius, GRAB_FILTER, &info);
if (shape && cpBodyGetMass(cpShapeGetBody(shape)) < INFINITY) {
cpVect nearest = (info.distance > 0.0f ? info.point : mouse_pos);
cpBody* body = cpShapeGetBody(shape);
state->mouse_joint =
cpPivotJointNew2(state->mouse_body, body, cpvzero,
cpBodyWorldToLocal(body, nearest));
cpConstraintSetMaxForce(state->mouse_joint, 5000000.0f);
cpConstraintSetErrorBias(state->mouse_joint,
cpfpow(1.0f - 0.15f, 60.0f));
cpSpaceAddConstraint(state->space, state->mouse_joint);
}
}
if (m.left_click == false && state->mouse_joint != NULL) {
cpSpaceRemoveConstraint(state->space, state->mouse_joint);
cpConstraintFree(state->mouse_joint);
state->mouse_joint = NULL;
}
}
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].point1 = (swapped ? p2 : p1);
set.points[i].point2 = (swapped ? p1 : p2);
set.points[i].distance = cpvdot(cpvsub(p2, p1), n);
}
return set;
}
static void
preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
{
CONSTRAINT_BEGIN(spring, a, b);
spring->r1 = cpvrotate(spring->anchr1, a->rot);
spring->r2 = cpvrotate(spring->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
cpFloat dist = cpvlength(delta);
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n);
spring->nMass = 1.0f/k;
spring->target_vrn = 0.0f;
spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k);
// apply spring force
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
}
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
preStep(cpPivotJoint *joint, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
// 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);
}
static void
preStep(cpSlideJoint *joint, cpFloat dt, cpFloat dt_inv)
{
CONSTRAINT_BEGIN(joint, a, b);
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
cpFloat dist = cpvlength(delta);
cpFloat pdist = 0.0f;
if(dist > joint->max) {
pdist = dist - joint->max;
} else if(dist < joint->min) {
pdist = joint->min - dist;
dist = -dist;
}
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)CP_INFINITY));
// calculate mass normal
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
// calculate bias velocity
cpFloat maxBias = joint->constraint.maxBias;
joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias);
// compute max impulse
joint->jnMax = J_MAX(joint, dt);
// apply accumulated impulse
if(!joint->bias) //{
// if bias is 0, then the joint is not at a limit.
joint->jnAcc = 0.0f;
// } else {
cpVect j = cpvmult(joint->n, joint->jnAcc);
apply_impulses(a, b, joint->r1, joint->r2, j);
// }
}
// takes array of vertices, returns center x and y coordinates in address of x and y
// algorithm: take average x value average y value
static void core_freestyle_center ( cpVect * verts, const int num_verts, cpVect * center ) {
cpFloat tot_length = 0.0;
// assume uniform mass, so length can be substitued as mass
cpVect center_of_mass = cpvzero;
for ( int i = 0; i< num_verts - 1; i++ ) {
cpFloat length = cpvdist ( verts[i], verts[i+1] );
cpVect to_cent = cpvmult ( cpvsub ( verts[i+1], verts[i] ), 0.5 );
cpVect loc_center = cpvadd ( verts[i], to_cent );
center_of_mass = cpvadd ( center_of_mass, cpvmult ( loc_center, length ) );
tot_length += length;
}
center_of_mass = cpvmult ( center_of_mass, 1/tot_length );
center->x = center_of_mass.x;
center->y = center_of_mass.y;
return;
}
static void
setUpVerts(cpPolyShape *poly, int numVerts, const cpVect *verts, cpVect offset)
{
// Fail if the user attempts to pass a concave poly, or a bad winding.
cpAssertHard(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding. Consider using cpConvexHull() or CP_CONVEX_HULL().");
poly->numVerts = numVerts;
poly->verts = (cpVect *)cpcalloc(2*numVerts, sizeof(cpVect));
poly->planes = (cpSplittingPlane *)cpcalloc(2*numVerts, sizeof(cpSplittingPlane));
poly->tVerts = poly->verts + numVerts;
poly->tPlanes = poly->planes + numVerts;
for(int i=0; i<numVerts; i++) {
cpVect a = cpvadd(offset, verts[i]);
cpVect b = cpvadd(offset, verts[(i+1)%numVerts]);
cpVect n = cpvnormalize(cpvperp(cpvsub(b, a)));
poly->verts[i] = a;
poly->planes[i].n = n;
poly->planes[i].d = cpvdot(n, a);
}
}
static void
preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
spring->r1 = cpvrotate(spring->anchr1, a->rot);
spring->r2 = cpvrotate(spring->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
cpFloat dist = cpvlength(delta);
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
// calculate mass normal
spring->nMass = 1.0f/k_scalar(a, b, spring->r1, spring->r2, spring->n);
spring->dt = dt;
spring->target_vrn = 0.0f;
// apply spring force
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
}
static void
update(int ticks)
{
int steps = 1;
cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
for(int i=0; i<steps; i++){
// turn the control body based on the angle relative to the actual body
cpVect mouseDelta = cpvsub(mousePoint, tankBody->p);
cpFloat turn = cpvtoangle(cpvunrotate(tankBody->rot, mouseDelta));
cpBodySetAngle(tankControlBody, tankBody->a - turn);
// drive the tank towards the mouse
if(cpvnear(mousePoint, tankBody->p, 30.0)){
tankControlBody->v = cpvzero; // stop
} else {
cpFloat direction = (cpvdot(mouseDelta, tankBody->rot) > 0.0 ? 1.0 : -1.0);
tankControlBody->v = cpvrotate(tankBody->rot, cpv(30.0f*direction, 0.0f));
}
cpSpaceStep(space, dt);
}
}
// 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
preStep(cpPinJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
cpFloat dist = cpvlength(delta);
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
// calculate mass normal
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
// calculate bias velocity
cpFloat maxBias = joint->constraint.maxBias;
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias);
// compute max impulse
joint->jnMax = J_MAX(joint, dt);
}
static void
preStep(cpDampedSpring *spring, cpFloat dt)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
spring->r1 = cpvrotate(spring->anchr1, a->rot);
spring->r2 = cpvrotate(spring->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
cpFloat dist = cpvlength(delta);
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n);
cpAssertSoft(k != 0.0, "Unsolvable spring.");
spring->nMass = 1.0f/k;
spring->target_vrn = 0.0f;
spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k);
// apply spring force
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
}
static void
display(void)
{
cpVect newPoint = cpvlerp(mousePoint_last, mousePoint, 0.25f);
mouseBody->p = newPoint;
mouseBody->v = cpvmult(cpvsub(newPoint, mousePoint_last), 60.0f);
mousePoint_last = newPoint;
if(!paused || step > 0){
currDemo->updateFunc(ticks);
step = (step > 1 ? step - 1 : 0);
}
glClear(GL_COLOR_BUFFER_BIT);
drawSpace(space, currDemo->drawOptions ? currDemo->drawOptions : &options);
if(!paused){
drawInstructions();
drawInfo();
}
drawString(-300, -210, messageString);
glutSwapBuffers();
ticks++;
}
void
cpBodySlew(cpBody *body, cpVect pos, cpFloat dt)
{
cpVect delta = cpvsub(pos, body->p);
body->v = cpvmult(delta, 1.0/dt);
}
cpVect * bmx_cpvect_sub(cpVect * vec, cpVect * vec2) {
return bmx_cpvect_new(cpvsub(*vec, *vec2));
}
// Modified from chipmunk_private.h
cpFloat Buoyancy::KScalarBody(cpBody *body, cpVect point, cpVect n)
{
cpFloat rcn = cpvcross(cpvsub(point, cpBodyGetPosition(body)), n);
return 1.0f/cpBodyGetMass(body) + rcn*rcn/cpBodyGetMoment(body);
}
void ColliderDetector::updateTransform(Mat4 &t)
{
if (!_active)
{
return;
}
for(auto& object : _colliderBodyList)
{
ColliderBody *colliderBody = (ColliderBody *)object;
ContourData *contourData = colliderBody->getContourData();
#if ENABLE_PHYSICS_BOX2D_DETECT
b2PolygonShape *shape = nullptr;
if (_body != nullptr)
{
shape = (b2PolygonShape *)colliderBody->getB2Fixture()->GetShape();
}
#elif ENABLE_PHYSICS_CHIPMUNK_DETECT
cpPolyShape *shape = nullptr;
if (_body != nullptr)
{
shape = (cpPolyShape *)colliderBody->getShape();
}
#endif
unsigned long num = contourData->vertexList.size();
std::vector<cocos2d::Vec2> &vs = contourData->vertexList;
#if ENABLE_PHYSICS_SAVE_CALCULATED_VERTEX
std::vector<cocos2d::Vec2> &cvs = colliderBody->_calculatedVertexList;
#endif
for (unsigned long i = 0; i < num; i++)
{
helpPoint.setPoint( vs.at(i).x, vs.at(i).y);
helpPoint = PointApplyTransform(helpPoint, t);
#if ENABLE_PHYSICS_SAVE_CALCULATED_VERTEX
cvs.at(i).x = helpPoint.x;
cvs.at(i).y = helpPoint.y;
#endif
#if ENABLE_PHYSICS_BOX2D_DETECT
if (shape != nullptr)
{
b2Vec2 &bv = shape->m_vertices[i];
bv.Set(helpPoint.x / PT_RATIO, helpPoint.y / PT_RATIO);
}
#elif ENABLE_PHYSICS_CHIPMUNK_DETECT
if (shape != nullptr)
{
cpVect v ;
v.x = helpPoint.x;
v.y = helpPoint.y;
shape->verts[i] = v;
}
#endif
}
#if ENABLE_PHYSICS_CHIPMUNK_DETECT
cpConvexHull((int)num, shape->verts, nullptr, nullptr, 0);
for (unsigned long i = 0; i < num; i++)
{
cpVect b = shape->verts[(i + 1) % shape->numVerts];
cpVect n = cpvnormalize(cpvperp(cpvsub(b, shape->verts[i])));
shape->planes[i].n = n;
shape->planes[i].d = cpvdot(n, shape->verts[i]);
}
#endif
}
}
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;
}
// 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);
}
// 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);
}
}
}
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)
{
static inline struct MinkowskiPoint
MinkowskiPointNew(const struct SupportPoint a, const struct SupportPoint b)
{
struct MinkowskiPoint point = {a.p, b.p, cpvsub(b.p, a.p), (a.index & 0xFF)<<8 | (b.index & 0xFF)};
return point;
}
static void update(void) {
int steps, i;
cpFloat dt;
cpVect look, lastPos, curPos;
projectile_t *pj; /* temp for looping */
static cpVect delta = {0, 0};
static projectile_t *curProj = NULL;
static bool canToggleEditor = true;
curPos = atlMousePos();
lastPos = atlMouseLastPos();
/* rotate 'gun' based on mouse position */
look = cpvnormalize(cpvsub(curPos, g_Cannon->body->p));
cpBodySetAngle(g_Cannon->body, cpvtoangle(look));
if (isKeyPressed(KEY_e) && canToggleEditor) {
canToggleEditor = false;
editorMode = !editorMode;
} else if (!isKeyPressed(KEY_e)) {
canToggleEditor = true;
}
if (editorMode) {
if (!editorBody)
initializeEditor();
handleEditor();
} else {
if (editorBody)
destroyEditor();
if (atlLeftMouseDown()) {
if (!curProj) {
/* don't let player hold the left mouse button to fire multiple rounds */
delta = cpvnormalize(cpvsub(atlMouseClickPos(), g_Cannon->body->p));
if (g_Cannon->ai < MAX_PROJECTILES) {
curProj = g_Cannon->ammo[g_Cannon->ai++];
if (curProj) {
cpSpaceAddBody(g_Space, curProj->body);
curProj->body->p = cpvadd(g_Cannon->body->p,
cpvmult(look, g_Cannon->length));
cpSpaceAddShape(g_Space, curProj->shape);
cpBodyApplyImpulse(curProj->body,
cpvmult(cpvforangle(g_Cannon->body->a), 600.0f),
cpvzero);
cpBodyActivate(curProj->body);
}
}
}
} else {
curProj = NULL;
}
}
/* treat projectiles as limited resources. only allocated at the start of a level */
for (i = 0; i <= g_Cannon->ai-1; ++i) {
pj = g_Cannon->ammo[i];
if (!pj)
continue;
if (pj->body->v.x >= -0.01f && pj->body->v.x <= 0.01f &&
pj->body->v.y >= -0.01f && pj->body->v.y <= 0.01f) {
/* TODO: mark an object for deletion, but don't delete immediately */
cpSpaceRemoveBody(g_Space, pj->body);
cpSpaceRemoveShape(g_Space, pj->shape);
free(g_Cannon->ammo[i]);
g_Cannon->ammo[i] = NULL;
}
}
steps = 3;
dt = 1.0f/60.0f/(cpFloat)steps;
for (i = 0; i < steps; ++i) {
cpSpaceStep(g_Space, dt);
}
}
void ColliderDetector::updateTransform(AffineTransform &t)
{
if (!m_bActive)
{
return;
}
for(auto object : *m_pColliderBodyList)
{
ColliderBody *colliderBody = (ColliderBody *)object;
ContourData *contourData = colliderBody->getContourData();
b2PolygonShape* b2shape = nullptr;
if (m_pB2Body != nullptr)
{
b2shape = (b2PolygonShape *)colliderBody->getB2Fixture()->GetShape();
}
cpPolyShape* cpshape = nullptr;
if (m_pCPBody != nullptr)
{
cpshape = (cpPolyShape *)colliderBody->getShape();
}
int num = contourData->m_tVertexList.count();
ContourVertex2 **vs = (ContourVertex2 **)contourData->m_tVertexList.data->arr;
#if ENABLE_PHYSICS_SAVE_CALCULATED_VERTEX
ContourVertex2 **cvs = (ContourVertex2 **)colliderBody->getCalculatedVertexList()->data->arr;
#endif
for (int i = 0; i < num; i++)
{
helpPoint.setPoint( vs[i]->x, vs[i]->y);
helpPoint = PointApplyAffineTransform(helpPoint, t);
#if ENABLE_PHYSICS_SAVE_CALCULATED_VERTEX
cvs[i]->x = helpPoint.x;
cvs[i]->y = helpPoint.y;
#endif
if ( b2shape != nullptr )
{
b2Vec2 &bv = b2shape->m_vertices[i];
bv.Set(helpPoint.x / PT_RATIO, helpPoint.y / PT_RATIO);
}
if ( cpshape != nullptr )
{
cpVect v ;
v.x = helpPoint.x;
v.y = helpPoint.y;
cpshape->verts[i] = v;
}
}
if ( cpshape != nullptr )
{
cpConvexHull(num, cpshape->verts, nullptr, nullptr, 0);
for (int i = 0; i < num; i++)
{
cpVect b = cpshape->verts[(i + 1) % cpshape->numVerts];
cpVect n = cpvnormalize(cpvperp(cpvsub(b, cpshape->verts[i])));
cpshape->planes[i].n = n;
cpshape->planes[i].d = cpvdot(n, cpshape->verts[i]);
}
}
}
}
static inline cpFloat
Sharpness(cpVect a, cpVect b, cpVect c)
{
// TODO could speed this up by caching the normals instead of calculating each twice.
return cpvdot(cpvnormalize(cpvsub(a, b)), cpvnormalize(cpvsub(c, b)));
}
