static cpBool
cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p){
if(!cpBBContainsVect(seg->shape.bb, p)) return cpFalse;
// 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;
}
bool DropCapsid::onContactAstroObj(ContactInfo& cinfo)
{
if (cinfo.contact.getEventCode() == PhysicsContact::EventCode::POSTSOLVE) {
if (cpArbiter* arb = static_cast<cpArbiter*>(cinfo.contact.getContactInfo())) {
cpContactPointSet cps = cpArbiterGetContactPointSet(arb);
bool moving = false;
for (int i = 0; i < cps.count; ++i) {
cpVect va = cpBodyGetVelocityAtWorldPoint(arb->body_a, cps.points[i].pointA);
cpVect vb = cpBodyGetVelocityAtWorldPoint(arb->body_b, cps.points[i].pointB);
cpVect rv = cpvsub(va, vb);
cpFloat wa = cpBodyGetAngularVelocity(arb->body_a);
cpFloat wb = cpBodyGetAngularVelocity(arb->body_b);
cpFloat rw = wa - wb;
//if (cpvlengthsq(rv) > 1e-6 || fabs(rw) > 0.5) {
if (cpvlengthsq(rv) > 1.0 || fabs(rw) > 5) {
moving = true;
break;
}
}
if (!moving) {
Unit* created = onLandCreate(_game);
Player* player = _player;
replaceWith(created); // this is destroyed
float up = (cinfo.thisBody->getPosition() - cinfo.thatBody->getPosition()).getAngle() / (M_PI / 180.0);
created->getNode()->getPhysicsBody()->setRotation(90 - up);
created->setPlayer(player);
// CCLOG("LANDING up# %f", up);
return true;
}
}
}
return true;
}
cpFloat
cpMomentForSegment(cpFloat m, cpVect a, cpVect b, cpFloat radius)
{
// TODO account for radius
cpVect offset = cpvmult(cpvadd(a, b), 0.5f);
return m*(cpvdistsq(b, a)/12.0f + cpvlengthsq(offset));
}
static int
circle2segment(const cpCircleShape *circleShape, const cpSegmentShape *segmentShape, cpContact *con)
{
cpVect seg_a = segmentShape->ta;
cpVect seg_b = segmentShape->tb;
cpVect center = circleShape->tc;
cpVect seg_delta = cpvsub(seg_b, seg_a);
cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(center, seg_a))/cpvlengthsq(seg_delta));
cpVect closest = cpvadd(seg_a, cpvmult(seg_delta, closest_t));
if(circle2circleQuery(center, closest, circleShape->r, segmentShape->r, con)){
cpVect n = con[0].n;
// Reject endcap collisions if tangents are provided.
if(
(closest_t == 0.0f && cpvdot(n, segmentShape->a_tangent) < 0.0) ||
(closest_t == 1.0f && cpvdot(n, segmentShape->b_tangent) < 0.0)
) return 0;
return 1;
} else {
return 0;
}
}
cpFloat
cpMomentForSegment(cpFloat m, cpVect a, cpVect b)
{
cpFloat length = cpvlength(cpvsub(b, a));
cpVect offset = cpvmult(cpvadd(a, b), 1.0f/2.0f);
return m*(length*length/12.0f + cpvlengthsq(offset));
}
cpFloat
cpMomentForSegment(cpFloat m, cpVect a, cpVect b, cpFloat r)
{
cpVect offset = cpvlerp(a, b, 0.5f);
// This approximates the shape as a box for rounded segments, but it's quite close.
cpFloat length = cpvdist(b, a) + 2.0f*r;
return m*((length*length + 4.0f*r*r)/12.0f + cpvlengthsq(offset));
}
void PhysicsForceField::addBodyField(cpVect p, PhysicsBody* body, float mass, cpVect& ret)
{
cpBody* cpBody = body->getCPBody();
cpVect d = cpvsub(cpBodyGetPosition(cpBody), p);
float dlensq = cpvlengthsq(d);
if (dlensq >= _minDistanceSq) {
ret = cpvadd(ret, cpvmult(d, mass / (dlensq*cpfsqrt(dlensq)) ));
}
}
cpFloat
cpMomentForBox2(cpFloat m, cpBB box)
{
cpFloat width = box.r - box.l;
cpFloat height = box.t - box.b;
cpVect offset = cpvmult(cpv(box.l + box.r, box.b + box.t), 0.5f);
return cpMomentForBox(m, width, height) + m*cpvlengthsq(offset);
}
cpFloat
cpMomentForBox2(cpFloat m, cpBB box)
{
cpFloat width = box.r - box.l;
cpFloat height = box.t - box.b;
cpVect offset = cpvmult(cpv(box.l + box.r, box.b + box.t), 0.5f);
// TODO NaN when offset is 0 and m is INFINITY
return cpMomentForBox(m, width, height) + m*cpvlengthsq(offset);
}
static void
planetGravityVelocityFunc(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
{
// Gravitational acceleration is proportional to the inverse square of
// distance, and directed toward the origin. The central planet is assumed
// to be massive enough that it affects the satellites but not vice versa.
cpVect p = cpBodyGetPosition(body);
cpFloat sqdist = cpvlengthsq(p);
cpVect g = cpvmult(p, -gravityStrength / (sqdist * cpfsqrt(sqdist)));
cpBodyUpdateVelocity(body, g, damping, dt);
}
static void
cpCircleShapePointQuery(cpCircleShape *circle, cpVect p, cpPointQueryExtendedInfo *info){
cpVect delta = cpvsub(p, circle->tc);
cpFloat distsq = cpvlengthsq(delta);
cpFloat r = circle->r;
if(distsq < r*r){
info->shape = (cpShape *)circle;
cpFloat dist = cpfsqrt(distsq);
info->d = r - dist;
info->n = cpvmult(delta, 1.0/dist);
}
}
// Submitted by LegoCyclon
static int
seg2seg(const cpShape* shape1, const cpShape* shape2, cpContact* con)
{
cpSegmentShape* seg1 = (cpSegmentShape *)shape1;
cpSegmentShape* seg2 = (cpSegmentShape *)shape2;
cpVect v1 = cpvsub(seg1->tb, seg1->ta);
cpVect v2 = cpvsub(seg2->tb, seg2->ta);
cpFloat v1lsq = cpvlengthsq(v1);
cpFloat v2lsq = cpvlengthsq(v2);
// project seg2 onto seg1
cpVect p1a = cpvproject(cpvsub(seg2->ta, seg1->ta), v1);
cpVect p1b = cpvproject(cpvsub(seg2->tb, seg1->ta), v1);
// project seg1 onto seg2
cpVect p2a = cpvproject(cpvsub(seg1->ta, seg2->ta), v2);
cpVect p2b = cpvproject(cpvsub(seg1->tb, seg2->ta), v2);
// clamp projections to segment endcaps
if (cpvdot(p1a, v1) < 0.0f)
p1a = cpvzero;
else if (cpvdot(p1a, v1) > 0.0f && cpvlengthsq(p1a) > v1lsq)
p1a = v1;
if (cpvdot(p1b, v1) < 0.0f)
p1b = cpvzero;
else if (cpvdot(p1b, v1) > 0.0f && cpvlengthsq(p1b) > v1lsq)
p1b = v1;
if (cpvdot(p2a, v2) < 0.0f)
p2a = cpvzero;
else if (cpvdot(p2a, v2) > 0.0f && cpvlengthsq(p2a) > v2lsq)
p2a = v2;
if (cpvdot(p2b, v2) < 0.0f)
p2b = cpvzero;
else if (cpvdot(p2b, v2) > 0.0f && cpvlengthsq(p2b) > v2lsq)
p2b = v2;
p1a = cpvadd(p1a, seg1->ta);
p1b = cpvadd(p1b, seg1->ta);
p2a = cpvadd(p2a, seg2->ta);
p2b = cpvadd(p2b, seg2->ta);
int num = 0;
if (!circle2circleQuery(p1a, p2a, seg1->r, seg2->r, nextContactPoint(con, &num)))
--num;
if (!circle2circleQuery(p1b, p2b, seg1->r, seg2->r, nextContactPoint(con, &num)))
--num;
if (!circle2circleQuery(p1a, p2b, seg1->r, seg2->r, nextContactPoint(con, &num)))
--num;
if (!circle2circleQuery(p1b, p2a, seg1->r, seg2->r, nextContactPoint(con, &num)))
--num;
return num;
}
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);
}
}
// 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;
}
// 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 inline cpFloat
ClosestDist(const cpVect v0,const cpVect v1)
{
return cpvlengthsq(LerpT(v0, v1, ClosestT(v0, v1)));
}
// 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);
}
cpFloat
cpMomentForCircle(cpFloat m, cpFloat r1, cpFloat r2, cpVect offset)
{
return m*(0.5f*(r1*r1 + r2*r2) + cpvlengthsq(offset));
}
cpFloat
cpMomentForSegment(cpFloat m, cpVect a, cpVect b)
{
cpVect offset = cpvmult(cpvadd(a, b), 0.5f);
return m*(cpvdistsq(b, a)/12.0f + cpvlengthsq(offset));
}
cpFloat bmx_cpvect_lengthsq(cpVect * vec) {
return cpvlengthsq(*vec);
}