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
preStep(cpRotaryLimitJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	CONSTRAINT_BEGIN(joint, a, b);
	
	cpFloat dist = b->a - a->a;
	cpFloat pdist = 0.0f;
	if(dist > joint->max) {
		pdist = joint->max - dist;
	} else if(dist < joint->min) {
		pdist = joint->min - dist;
	}
	
	// calculate moment of inertia coefficient.
	joint->iSum = 1.0f/(a->i_inv + b->i_inv);
	
	// calculate bias velocity
	cpFloat maxBias = joint->constraint.maxBias;
	joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(pdist), -maxBias, maxBias);
	
	// compute max impulse
	joint->jMax = J_MAX(joint, dt);

	// If the bias is 0, the joint is not at a limit. Reset the impulse.
	if(!joint->bias)
		joint->jAcc = 0.0f;

	// apply joint torque
	a->w -= joint->jAcc*a->i_inv;
	b->w += joint->jAcc*b->i_inv;
}
Esempio n. 3
0
static void
preStep(cpSimpleMotor *joint, cpFloat dt, cpFloat dt_inv)
{
	CONSTRAINT_BEGIN(joint, a, b);
	
	// calculate moment of inertia coefficient.
	joint->iSum = 1.0f/(a->i_inv + b->i_inv);
	
	// compute max impulse
	joint->jMax = J_MAX(joint, dt);

	// apply joint torque
	a->w -= joint->jAcc*a->i_inv;
	b->w += joint->jAcc*b->i_inv;
}
static void
preStep(cpDampedRotarySpring *spring, cpFloat dt, cpFloat dt_inv)
{
	CONSTRAINT_BEGIN(spring, a, b);
	
	cpFloat moment = a->i_inv + b->i_inv;
	spring->iSum = 1.0f/moment;

	spring->w_coef = 1.0f - cpfexp(-spring->damping*dt*moment);
	spring->target_wrn = 0.0f;

	// apply spring torque
	cpFloat j_spring = spring->springTorqueFunc((cpConstraint *)spring, a->a - b->a)*dt;
	a->w -= j_spring*a->i_inv;
	b->w += j_spring*b->i_inv;
}
static void
applyImpulse(cpGearJoint *joint)
{
	CONSTRAINT_BEGIN(joint, a, b);
	
	// compute relative rotational velocity
	cpFloat wr = b->w*joint->ratio - a->w;
	
	// compute normal impulse	
	cpFloat j = (joint->bias - wr)*joint->iSum;
	cpFloat jOld = joint->jAcc;
	joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax);
	j = joint->jAcc - jOld;
	
	// apply impulse
	a->w -= j*a->i_inv*joint->ratio_inv;
	b->w += j*b->i_inv;
}
Esempio n. 6
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. 7
0
static void
applyImpulse(cpSimpleMotor *joint)
{
	CONSTRAINT_BEGIN(joint, a, b);
	
	// compute relative rotational velocity
	cpFloat wr = b->w - a->w + joint->rate;
	
	// compute normal impulse	
	cpFloat j = -wr*joint->iSum;
	cpFloat jOld = joint->jAcc;
	joint->jAcc = cpfclamp(jOld + j, -joint->jMax, joint->jMax);
	j = joint->jAcc - jOld;
	
	// apply impulse
	a->w -= j*a->i_inv;
	b->w += j*b->i_inv;
}
static void
applyImpulse(cpDampedRotarySpring *spring)
{
	CONSTRAINT_BEGIN(spring, a, b);
	
	// compute relative velocity
	cpFloat wrn = a->w - b->w;//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 w_damp = wrn*spring->w_coef;
	spring->target_wrn = wrn - w_damp;
	
	//apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
	cpFloat j_damp = w_damp*spring->iSum;
	a->w -= j_damp*a->i_inv;
	b->w += j_damp*b->i_inv;
}
Esempio n. 9
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. 10
0
static void
preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	CONSTRAINT_BEGIN(joint, a, 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. 11
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. 12
0
static void
preStep(cpGearJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	CONSTRAINT_BEGIN(joint, a, b);
	
	// calculate moment of inertia coefficient.
	joint->iSum = 1.0f/(a->i_inv*joint->ratio_inv + joint->ratio*b->i_inv);
	
	// calculate bias velocity
	cpFloat maxBias = joint->constraint.maxBias;
	joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*(b->a*joint->ratio - a->a - joint->phase), -maxBias, maxBias);
	
	// compute max impulse
	joint->jMax = J_MAX(joint, dt);

	// apply joint torque
	cpFloat j = joint->jAcc;
	a->w -= j*a->i_inv*joint->ratio_inv;
	b->w += j*b->i_inv;
}
Esempio n. 13
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. 14
0
static void
applyImpulse(cpRatchetJoint *joint)
{
	if(!joint->bias) return; // early exit

	CONSTRAINT_BEGIN(joint, a, b);
	
	// compute relative rotational velocity
	cpFloat wr = b->w - a->w;
	cpFloat ratchet = joint->ratchet;
	
	// compute normal impulse	
	cpFloat j = -(joint->bias + wr)*joint->iSum;
	cpFloat jOld = joint->jAcc;
	joint->jAcc = cpfclamp((jOld + j)*ratchet, 0.0f, joint->jMax*cpfabs(ratchet))/ratchet;
	j = joint->jAcc - jOld;
	
	// apply impulse
	a->w -= j*a->i_inv;
	b->w += j*b->i_inv;
}
Esempio n. 15
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. 16
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. 17
0
static void
preStep(cpSlideJoint *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);
	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)CP_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. 18
0
static void
preStep(cpRatchetJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	CONSTRAINT_BEGIN(joint, a, b);
	
	cpFloat angle = joint->angle;
	cpFloat phase = joint->phase;
	cpFloat ratchet = joint->ratchet;
	
	cpFloat delta = b->a - a->a;
	cpFloat diff = angle - delta;
	cpFloat pdist = 0.0f;
	
	if(diff*ratchet > 0.0f){
		pdist = diff;
	} else {
		joint->angle = cpffloor((delta - phase)/ratchet)*ratchet + phase;
	}
	
	// calculate moment of inertia coefficient.
	joint->iSum = 1.0f/(a->i_inv + b->i_inv);
	
	// calculate bias velocity
	cpFloat maxBias = joint->constraint.maxBias;
	joint->bias = cpfclamp(-joint->constraint.biasCoef*dt_inv*pdist, -maxBias, maxBias);
	
	// compute max impulse
	joint->jMax = J_MAX(joint, dt);

	// If the bias is 0, the joint is not at a limit. Reset the impulse.
	if(!joint->bias)
		joint->jAcc = 0.0f;

	// apply joint torque
	a->w -= joint->jAcc*a->i_inv;
	b->w += joint->jAcc*b->i_inv;
}
Esempio n. 19
0
static void
applyImpulse(cpRotaryLimitJoint *joint)
{
	if(!joint->bias) return; // early exit

	CONSTRAINT_BEGIN(joint, a, b);
	
	// compute relative rotational velocity
	cpFloat wr = b->w - a->w;
	
	// compute normal impulse	
	cpFloat j = -(joint->bias + wr)*joint->iSum;
	cpFloat jOld = joint->jAcc;
	if(joint->bias < 0.0f){
		joint->jAcc = cpfclamp(jOld + j, 0.0f, joint->jMax);
	} else {
		joint->jAcc = cpfclamp(jOld + j, -joint->jMax, 0.0f);
	}
	j = joint->jAcc - jOld;
	
	// apply impulse
	a->w -= j*a->i_inv;
	b->w += j*b->i_inv;
}
Esempio n. 20
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));
}