Esempio n. 1
0
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);
}
Esempio n. 2
0
static void
applyImpulse(cpSlideJoint *joint)
{
	if(cpveql(joint->n, cpvzero)) return;  // early exit

	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	
	cpVect n = joint->n;
	cpVect r1 = joint->r1;
	cpVect r2 = joint->r2;
		
	// compute relative velocity
	cpVect vr = relative_velocity(a, b, r1, r2);
	cpFloat vrn = cpvdot(vr, n);
	
	// compute normal impulse
	cpFloat jn = (joint->bias - vrn)*joint->nMass;
	cpFloat jnOld = joint->jnAcc;
	joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, 0.0f);
	jn = joint->jnAcc - jnOld;
	
	// apply impulse
	apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
}
Esempio n. 3
0
static void
applyCachedImpulse(cpPivotJoint *joint, cpFloat dt_coef)
{
	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	
	apply_impulses(a, b, joint->r1, joint->r2, cpvmult(joint->jAcc, dt_coef));
}
Esempio n. 4
0
static void
applyCachedImpulse(cpSlideJoint *joint, cpFloat dt_coef)
{
	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	
	cpVect j = cpvmult(joint->n, joint->jnAcc*dt_coef);
	apply_impulses(a, b, joint->r1, joint->r2, j);
}
Esempio n. 5
0
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));
	}
}
Esempio n. 6
0
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)));
	}
}
Esempio n. 7
0
void
cpArbiterApplyCachedImpulse(cpArbiter *arb)
{
	cpShape *shapea = arb->a;
	cpShape *shapeb = arb->b;
		
	arb->u = shapea->u * shapeb->u;
	arb->surface_vr = cpvsub(shapeb->surface_v, shapea->surface_v);

	cpBody *a = shapea->body;
	cpBody *b = shapeb->body;
	
	for(int i=0; i<arb->numContacts; i++){
		cpContact *con = &arb->contacts[i];
		apply_impulses(a, b, con->r1, con->r2, cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc)));
	}
}
Esempio n. 8
0
static void
applyImpulse(cpPinJoint *joint)
{
	CONSTRAINT_BEGIN(joint, a, b);
	cpVect n = joint->n;

	// compute relative velocity
	cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n);
	
	// compute normal impulse
	cpFloat jn = (joint->bias - vrn)*joint->nMass;
	cpFloat jnOld = joint->jnAcc;
	joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax);
	jn = joint->jnAcc - jnOld;
	
	// apply impulse
	apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
}
Esempio n. 9
0
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);
}
Esempio n. 10
0
static void
applyImpulse(cpDampedSpring *spring)
{
	cpBody *a = spring->constraint.a;
	cpBody *b = spring->constraint.b;
	
	cpVect n = spring->n;
	cpVect r1 = spring->r1;
	cpVect r2 = spring->r2;

	// compute relative velocity
	cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n);
	
	// compute velocity loss from drag
	cpFloat v_damp = (spring->target_vrn - vrn)*spring->v_coef;
	spring->target_vrn = vrn + v_damp;
	
	apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
}
Esempio n. 11
0
static void
applyImpulse(cpGrooveJoint *joint)
{
	CONSTRAINT_BEGIN(joint, a, b);
	
	cpVect r1 = joint->r1;
	cpVect r2 = joint->r2;
	
	// compute impulse
	cpVect vr = relative_velocity(a, b, r1, r2);

	cpVect j = mult_k(cpvsub(joint->bias, vr), joint->k1, joint->k2);
	cpVect jOld = joint->jAcc;
	joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j));
	j = cpvsub(joint->jAcc, jOld);
	
	// apply impulse
	apply_impulses(a, b, joint->r1, joint->r2, j);
}
Esempio n. 12
0
static void
applyImpulse(cpDampedSpring *spring)
{
	CONSTRAINT_BEGIN(spring, a, b);
	
	cpVect n = spring->n;
	cpVect r1 = spring->r1;
	cpVect r2 = spring->r2;

	// compute relative velocity
	cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
	
	// compute velocity loss from drag
	// not 100% certain this is derived correctly, though it makes sense
	cpFloat v_damp = -vrn*spring->v_coef;
	spring->target_vrn = vrn + v_damp;
	
	apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
}
Esempio n. 13
0
static void
applyImpulse(cpGrooveJoint *joint, cpFloat dt)
{
	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	
	cpVect r1 = joint->r1;
	cpVect r2 = joint->r2;
	
	// compute impulse
	cpVect vr = relative_velocity(a, b, r1, r2);

	cpVect j = cpMat2x2Transform(joint->k, cpvsub(joint->bias, vr));
	cpVect jOld = joint->jAcc;
	joint->jAcc = grooveConstrain(joint, cpvadd(jOld, j), dt);
	j = cpvsub(joint->jAcc, jOld);
	
	// apply impulse
	apply_impulses(a, b, joint->r1, joint->r2, j);
}
Esempio n. 14
0
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);
}
Esempio n. 15
0
static void
applyImpulse(cpDampedSpring *spring)
{
	cpBody *a = spring->constraint.a;
	cpBody *b = spring->constraint.b;
	
	cpVect n = spring->n;
	cpVect r1 = spring->r1;
	cpVect r2 = spring->r2;

	// compute relative velocity
	cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
	
	// compute velocity loss from drag
	// not 100% certain this is derived correctly, though it makes sense
	cpFloat v_damp = -vrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->nMass));
	spring->target_vrn = vrn + v_damp;
	
	apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
}
Esempio n. 16
0
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);
}
Esempio n. 17
0
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
applyImpulse(cpPinJoint *joint, cpFloat dt)
{
	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	cpVect n = joint->n;

	// compute relative velocity
	cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n);
	
	cpFloat jnMax = joint->constraint.maxForce*dt;
	
	// compute normal impulse
	cpFloat jn = (joint->bias - vrn)*joint->nMass;
	cpFloat jnOld = joint->jnAcc;
	joint->jnAcc = cpfclamp(jnOld + jn, -jnMax, jnMax);
	jn = joint->jnAcc - jnOld;
	
	// apply impulse
	apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
}
Esempio n. 19
0
void
cpArbiterApplyImpulse(cpArbiter *arb)
{
	cpBody *a = arb->body_a;
	cpBody *b = arb->body_b;
	cpVect surface_vr = arb->surface_vr;
	cpFloat friction = arb->u;

	for(int i=0; i<arb->numContacts; i++){
		cpContact *con = &arb->contacts[i];
		cpFloat nMass = con->nMass;
		cpVect n = con->n;
		cpVect r1 = con->r1;
		cpVect r2 = con->r2;

		cpVect vb1 = cpvadd(a->v_bias, cpvmult(cpvperp(r1), a->w_bias));
		cpVect vb2 = cpvadd(b->v_bias, cpvmult(cpvperp(r2), b->w_bias));
		cpVect vr = relative_velocity(a, b, r1, r2);

		cpFloat vbn = cpvdot(cpvsub(vb2, vb1), n);
		cpFloat vrn = cpvdot(vr, n);
		cpFloat vrt = cpvdot(cpvadd(vr, surface_vr), cpvperp(n));

		cpFloat jbn = (con->bias - vbn)*nMass;
		cpFloat jbnOld = con->jBias;
		con->jBias = cpfmax(jbnOld + jbn, 0.0f);

		cpFloat jn = -(con->bounce + vrn)*nMass;
		cpFloat jnOld = con->jnAcc;
		con->jnAcc = cpfmax(jnOld + jn, 0.0f);

		cpFloat jtMax = friction*con->jnAcc;
		cpFloat jt = -vrt*con->tMass;
		cpFloat jtOld = con->jtAcc;
		con->jtAcc = cpfclamp(jtOld + jt, -jtMax, jtMax);

		apply_bias_impulses(a, b, r1, r2, cpvmult(n, con->jBias - jbnOld));
		apply_impulses(a, b, r1, r2, cpvrotate(n, cpv(con->jnAcc - jnOld, con->jtAcc - jtOld)));
	}
}
Esempio n. 20
0
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));
}
Esempio n. 21
0
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);
//	}
}
Esempio n. 22
0
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));
}
Esempio n. 23
0
static void
applyImpulse(cpSlideJoint *joint)
{
	if(!joint->bias) return;  // early exit

	CONSTRAINT_BEGIN(joint, a, b);
	
	cpVect n = joint->n;
	cpVect r1 = joint->r1;
	cpVect r2 = joint->r2;
		
	// compute relative velocity
	cpVect vr = relative_velocity(a, b, r1, r2);
	cpFloat vrn = cpvdot(vr, n);
	
	// compute normal impulse
	cpFloat jn = (joint->bias - vrn)*joint->nMass;
	cpFloat jnOld = joint->jnAcc;
	joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, 0.0f);
	jn = joint->jnAcc - jnOld;
	
	// apply impulse
	apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
}
Esempio n. 24
0
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));
}
Esempio n. 25
0
void RaycastCar::updateVehicle( btScalar step )
{
    m_currentVehicleSpeedKmHour = btScalar(3.6f) * getRigidBody()->getLinearVelocity().length();
    const btTransform & chassisTrans = getChassisWorldTransform();

    btVector3 forwardW(chassisTrans.getBasis()[0][m_indexForwardAxis],
                       chassisTrans.getBasis()[1][m_indexForwardAxis],
                       chassisTrans.getBasis()[2][m_indexForwardAxis]);

    if (forwardW.dot(getRigidBody()->getLinearVelocity()) < btScalar(0.0f))
        m_currentVehicleSpeedKmHour *= btScalar(-1.0f);

    for (int i = 0; i < getNumWheels(); i++)
    {
        updateWheelTransform(i, false);
        btScalar depth;
        depth = rayCast(m_wheelInfo[i]);
    }

    update_suspension(step);
    update_engine(step);
    update_forces(step);
    apply_impulses(step);
}