void
cpDampedSpring(cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2, cpFloat rlen, cpFloat k, cpFloat dmp, cpFloat dt)
{
// Calculate the world space anchor coordinates.
cpVect r1 = cpvrotate(anchr1, a->rot);
cpVect r2 = cpvrotate(anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, r2), cpvadd(a->p, r1));
cpFloat dist = cpvlength(delta);
cpVect n = dist ? cpvmult(delta, 1.0f/dist) : cpvzero;
cpFloat f_spring = (dist - rlen)*k;
// Calculate the world relative velocities of the anchor points.
cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(r1), a->w));
cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(r2), b->w));
// Calculate the damping force.
// This really should be in the impulse solver and can produce problems when using large damping values.
cpFloat vrn = cpvdot(cpvsub(v2, v1), n);
cpFloat f_damp = vrn*cpfmin(dmp, 1.0f/(dt*(a->m_inv + b->m_inv)));
// Apply!
cpVect f = cpvmult(n, f_spring + f_damp);
cpBodyApplyForce(a, f, r1);
cpBodyApplyForce(b, cpvneg(f), r2);
}
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 > cp_collision_slop) ? cpvmult(joint->u1, 1.0f/length1) : cpvzero;
joint->u2 = (length2 > cp_collision_slop) ? 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 void
preStep(cpPinJoint *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);
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(-joint->constraint.biasCoef*dt_inv*(dist - joint->dist), -maxBias, maxBias);
// compute max impulse
joint->jnMax = J_MAX(joint, dt);
// apply accumulated impulse
cpVect j = cpvmult(joint->n, joint->jnAcc);
apply_impulses(a, b, joint->r1, joint->r2, j);
}
static cpBB
cpSegmentShapeCacheData(cpSegmentShape *seg, cpVect p, cpVect rot)
{
seg->ta = cpvadd(p, cpvrotate(seg->a, rot));
seg->tb = cpvadd(p, cpvrotate(seg->b, rot));
seg->tn = cpvrotate(seg->n, rot);
cpFloat l,r,b,t;
if(seg->ta.x < seg->tb.x){
l = seg->ta.x;
r = seg->tb.x;
} else {
l = seg->tb.x;
r = seg->ta.x;
}
if(seg->ta.y < seg->tb.y){
b = seg->ta.y;
t = seg->tb.y;
} else {
b = seg->tb.y;
t = seg->ta.y;
}
cpFloat rad = seg->r;
return cpBBNew(l - rad, b - rad, r + rad, t + rad);
}
static void
preStep(cpSlideJoint *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);
cpFloat pdist = 0.0f;
if(dist > joint->max) {
pdist = dist - joint->max;
joint->n = cpvnormalize_safe(delta);
} else if(dist < joint->min) {
pdist = joint->min - dist;
dist = -dist;
joint->n = cpvneg(cpvnormalize_safe(delta));
} else {
joint->n = cpvzero;
joint->jnAcc = 0.0f;
}
// 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)*pdist/dt, -maxBias, maxBias);
// compute max impulse
joint->jnMax = J_MAX(joint, dt);
}
static void
preStep(cpSlideJoint *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);
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)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)*pdist/dt, -maxBias, maxBias);
// compute max impulse
joint->jnMax = J_MAX(joint, dt);
// if bias is 0, then the joint is not at a limit. Reset cached impulse.
if(!joint->bias) joint->jnAcc = 0.0f;
}
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));
}
}
}
void CDynamics2DMultiBodyObjectModel::MoveTo(const CVector3& c_position,
const CQuaternion& c_orientation) {
/* Set target position and orientation */
cpVect tBodyPos = cpv(c_position.GetX(), c_position.GetY());
CRadians cXAngle, cYAngle, cZAngle;
c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle);
cpFloat tBodyOrient = cZAngle.GetValue();
/* For each body: */
for(size_t i = 0; i < m_vecBodies.size(); ++i) {
/* Set body orientation at anchor */
cpBodySetAngle(m_vecBodies[i].Body,
tBodyOrient + m_vecBodies[i].OffsetOrient);
/* Set body position at anchor */
cpBodySetPos(m_vecBodies[i].Body,
cpvadd(tBodyPos,
cpvrotate(m_vecBodies[i].OffsetPos,
m_vecBodies[i].Body->rot)));
/* Update shape index */
cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(),
m_vecBodies[i].Body);
}
/* Update ARGoS entity state */
UpdateEntityStatus();
}
void draw_shape(cpBody* body, cpShape* shape, void* data)
{
// get body info
cpVect v = cpBodyGetPos(body);
cpFloat angle = cpBodyGetAngle(body);
cpVect rot = cpvforangle(angle);
// get vectors
int n = cpPolyShapeGetNumVerts(shape);
SDL_Point* pts = calloc(sizeof(SDL_Point), n+1);
// rotate vectors
int i;
for(i=0; i<n; i++) {
cpVect p = cpPolyShapeGetVert(shape, i);
cpVect vr = cpvrotate(cpv(p.x,p.y), rot);
pts[i] = (SDL_Point) { (vr.x+v.x)*10+50, (vr.y+v.y)*10+50 };
if(i == 0)
pts[n] = pts[i];
}
// draw
SDL_RenderDrawLines(ren, pts, n+1);
free(pts);
}
static cpBB
cpCircleShapeCacheData(cpCircleShape *circle, cpVect p, cpVect rot)
{
cpVect c = circle->tc = cpvadd(p, cpvrotate(circle->c, rot));
cpFloat r = circle->r;
return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r);
}
void weapon_update(Entity *ent) {
cpBody *body = entity_body(ent);
WeaponEntityData *data = entity_data(ent);
if (data->owner && !data->player_joint) {
cvar_setd_player(entity_owner(ent), "weapon_id", entity_id(ent));
data->player_joint = cpSpaceAddConstraint(game.space, cpPivotJointNew2(
body,
entity_body(data->owner),
cpv(0, -WEAPON_HEIGHT / 2), cpv(1, 0)
));
cpConstraintSetErrorBias(data->player_joint, 0);
}
int mult = 0;
if (keymap_is_held("mouse1")) mult++;
if (keymap_is_held("mouse2")) mult--;
if (mult) {
cpVect force_pos = cpv(0, WEAPON_HEIGHT);
cpVect weapon_pos = cpBodyGetPosition(body);
cpVect world_force_pos = cpvadd(weapon_pos, cpvrotate(cpBodyGetRotation(body), force_pos));
cpVect mouse_delta = cpvsub(keymap_mouse_world(), world_force_pos);
cpVect force = cpvmult(cpvnormalize(mouse_delta), mult * WEAPON_SWING_FORCE);
cpBodyApplyForceAtWorldPoint(body, force, world_force_pos);
}
}
cpPinJoint *
cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
{
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
joint->anchr1 = anchr1;
joint->anchr2 = anchr2;
// STATIC_BODY_CHECK
cpVect p1 = (a ? cpvadd(a->p, cpvrotate(anchr1, a->rot)) : anchr1);
cpVect p2 = (b ? cpvadd(b->p, cpvrotate(anchr2, b->rot)) : anchr2);
joint->dist = cpvlength(cpvsub(p2, p1));
joint->jnAcc = 0.0f;
return joint;
}
static cpBB
cpCircleShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
{
cpCircleShape *circle = (cpCircleShape *)shape;
circle->tc = cpvadd(p, cpvrotate(circle->c, rot));
return bbFromCircle(circle->tc, circle->r);
}
static void
cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot)
{
cpVect *src = poly->verts;
cpVect *dst = poly->tVerts;
for(int i=0; i<poly->numVerts; i++)
dst[i] = cpvadd(p, cpvrotate(src[i], rot));
}
static void DrawConstraint(cpConstraint *constraint, DrawNode *renderer)
{
cpBody *body_a = cpConstraintGetBodyA(constraint);
cpBody *body_b = cpConstraintGetBodyB(constraint);
if(cpConstraintIsPinJoint(constraint))
{
cpVect a = cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpPinJointGetAnchorA(constraint), cpBodyGetRotation(body_a)));
cpVect b = cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpPinJointGetAnchorB(constraint), cpBodyGetRotation(body_b)));
renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
}
else if(cpConstraintIsSlideJoint(constraint))
{
cpVect a = cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpSlideJointGetAnchorA(constraint), cpBodyGetRotation(body_a)));
cpVect b = cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpSlideJointGetAnchorB(constraint), cpBodyGetRotation(body_b)));
renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
}
else if(cpConstraintIsPivotJoint(constraint))
{
cpVect a = cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpPivotJointGetAnchorA(constraint), cpBodyGetRotation(body_a)));
cpVect b = cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpPivotJointGetAnchorB(constraint), cpBodyGetRotation(body_b)));
renderer->drawDot(cpVert2Point(a), 3.0, CONSTRAINT_COLOR);
renderer->drawDot(cpVert2Point(b), 3.0, CONSTRAINT_COLOR);
}
else if(cpConstraintIsGrooveJoint(constraint))
{
cpVect a = cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpGrooveJointGetGrooveA(constraint), cpBodyGetRotation(body_a)));
cpVect b = cpvadd(cpBodyGetPosition(body_a), cpvrotate(cpGrooveJointGetGrooveB(constraint), cpBodyGetRotation(body_a)));
cpVect c = cpvadd(cpBodyGetPosition(body_b), cpvrotate(cpGrooveJointGetAnchorB(constraint), cpBodyGetRotation(body_b)));
renderer->drawDot(cpVert2Point(c), 3.0, CONSTRAINT_COLOR);
renderer->drawSegment(cpVert2Point(a), cpVert2Point(b), 1.0, CONSTRAINT_COLOR);
}
else if(cpConstraintIsDampedSpring(constraint))
{
// TODO: uninplemented
}
else
{
// printf("Cannot draw constraint\n");
}
}
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);
}
static void
preStep(cpPivotJoint *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);
// 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, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
}
cpPinJoint *
cpPinJointInit(cpPinJoint *joint, cpBody *a, cpBody *b, cpVect anchr1, cpVect anchr2)
{
cpConstraintInit((cpConstraint *)joint, &klass, a, b);
joint->anchr1 = anchr1;
joint->anchr2 = anchr2;
// STATIC_BODY_CHECK
cpVect p1 = (a ? cpvadd(a->p, cpvrotate(anchr1, a->rot)) : anchr1);
cpVect p2 = (b ? cpvadd(b->p, cpvrotate(anchr2, b->rot)) : anchr2);
joint->dist = cpvlength(cpvsub(p2, p1));
cpAssertWarn(joint->dist > 0.0, "You created a 0 length pin joint. A pivot joint will be much more stable.");
joint->jnAcc = 0.0f;
return joint;
}
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);
}
static void
cpPolyShapeTransformAxes(cpPolyShape *poly, cpVect p, cpVect rot)
{
cpPolyShapeAxis *src = poly->axes;
cpPolyShapeAxis *dst = poly->tAxes;
for(int i=0; i<poly->numVerts; i++){
cpVect n = cpvrotate(src[i].n, rot);
dst[i].n = n;
dst[i].d = cpvdot(p, n) + src[i].d;
}
}
static void
preStep(cpPivotJoint *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);
// 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(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));
}
cpVect
cpArbiterTotalImpulseWithFriction(cpArbiter *arb)
{
cpContact *contacts = arb->contacts;
cpVect sum = cpvzero;
for(int i=0, count=arb->numContacts; i<count; i++){
cpContact *con = &contacts[i];
sum = cpvadd(sum, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc)));
}
return sum;
}
cpVect
cpArbiterTotalImpulseWithFriction(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, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc)));
}
return (arb->swappedColl ? sum : cpvneg(sum));
}
static void
preStep(cpSlideJoint *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);
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)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);
// }
}
static void
preStep(cpGrooveJoint *joint, cpFloat dt)
{
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
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);
}
void
cpArbiterApplyImpulse(cpArbiter *arb, cpFloat eCoef)
{
cpBody *a = arb->a->body;
cpBody *b = arb->b->body;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
cpVect n = con->n;
cpVect r1 = con->r1;
cpVect r2 = con->r2;
// Calculate the relative bias velocities.
cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
// Calculate and clamp the bias impulse.
cpFloat jbn = (con->bias - vbn)*con->nMass;
cpFloat jbnOld = con->jBias;
con->jBias = cpfmax(jbnOld + jbn, 0.0f);
jbn = con->jBias - jbnOld;
// Apply the bias impulse.
apply_bias_impulses(a, b, r1, r2, cpvmult(n, jbn));
// Calculate the relative velocity.
cpVect vr = relative_velocity(a, b, r1, r2);
cpFloat vrn = cpvdot(vr, n);
// Calculate and clamp the normal impulse.
cpFloat jn = -(con->bounce*eCoef + vrn)*con->nMass;
cpFloat jnOld = con->jnAcc;
con->jnAcc = cpfmax(jnOld + jn, 0.0f);
jn = con->jnAcc - jnOld;
// Calculate the relative tangent velocity.
cpFloat vrt = cpvdot(cpvadd(vr, arb->surface_vr), cpvperp(n));
// Calculate and clamp the friction impulse.
cpFloat jtMax = arb->u*con->jnAcc;
cpFloat jt = -vrt*con->tMass;
cpFloat jtOld = con->jtAcc;
con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);
jt = con->jtAcc - jtOld;
// Apply the final impulse.
apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(jn, jt)));
}
}
void
cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef)
{
if(cpArbiterIsFirstContact(arb)) return;
cpBody *a = arb->body_a;
cpBody *b = arb->body_b;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
cpVect j = cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc));
apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef));
}
}
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
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));
}
