void cpBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) { body->v = cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)), body->v_limit); cpFloat w_limit = body->w_limit; body->w = cpfclamp(body->w*damping + body->t*body->i_inv*dt, -w_limit, w_limit); }
static void internalBodyUpdateVelocity(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt) { cpBodyUpdateVelocity(body, cpvzero, damping, dt); // Skip kinematic bodies. if(cpBodyGetType(body) == CP_BODY_TYPE_KINEMATIC) return; cpAssertSoft(body->m > 0.0f && body->i > 0.0f, "Body's mass and moment must be positive to simulate. (Mass: %f Moment: f)", body->m, body->i); cocos2d::PhysicsBody *physicsBody = static_cast<cocos2d::PhysicsBody*>(body->userData); if(physicsBody->isGravityEnabled()) body->v = cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvadd(gravity, cpvmult(body->f, body->m_inv)), dt)), physicsBody->getVelocityLimit()); else body->v = cpvclamp(cpvadd(cpvmult(body->v, damping), cpvmult(cpvmult(body->f, body->m_inv), dt)), physicsBody->getVelocityLimit()); cpFloat w_limit = physicsBody->getAngularVelocityLimit(); body->w = cpfclamp(body->w*damping + body->t*body->i_inv*dt, -w_limit, w_limit); // Reset forces. body->f = cpvzero; //to check body sanity cpBodySetTorque(body, 0.0f); }
static void preStep(cpPivotJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog)); joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog)); // 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); }
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); }
static void applyImpulse(cpPivotJoint *joint) { CONSTRAINT_BEGIN(joint, a, b); cpVect r1 = joint->r1; cpVect r2 = joint->r2; // compute relative velocity cpVect vr = relative_velocity(a, b, r1, r2); // compute normal impulse cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2); cpVect jOld = joint->jAcc; joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->jMaxLen); j = cpvsub(joint->jAcc, jOld); // apply impulse apply_impulses(a, b, joint->r1, joint->r2, j); }
static void applyImpulse(cpPivotJoint *joint, cpFloat dt) { cpBody *a = joint->constraint.a; cpBody *b = joint->constraint.b; cpVect r1 = joint->r1; cpVect r2 = joint->r2; // compute relative velocity cpVect vr = relative_velocity(a, b, r1, r2); // compute normal impulse cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr)); cpVect jOld = joint->jAcc; joint->jAcc = cpvclamp(cpvadd(joint->jAcc, j), joint->constraint.maxForce*dt); j = cpvsub(joint->jAcc, jOld); // apply impulse apply_impulses(a, b, joint->r1, joint->r2, j); }
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(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); }
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); }