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)); }