// Verify that it's possible to enable / disable collisions between particles.
TEST_F(BodyContactTests, EnableDisableParticleContactsWithContactFilter)
{
	m_world->SetGravity(b2Vec2_zero);

	// Through two particles horizontally at each other
	// (A moving right, B moving left).
	b2ParticleDef pd;
	pd.flags = b2_particleContactFilterParticle;
	pd.position.Set(-m_particleDiameter * 2.0f, 0.0f);
	pd.velocity.Set(m_particleDiameter, 0.0f);
	int32 particleIndexA = m_particleSystem->CreateParticle(pd);

	pd.flags = b2_particleContactFilterParticle;
	pd.position.Set(m_particleDiameter * 2.0f, 0.0f);
	pd.velocity.Set(-m_particleDiameter, 0.0f);
	int32 particleIndexB = m_particleSystem->CreateParticle(pd);

	// Leave contacts enabled.
	ParticleContactDisabler contactDisabler;
	m_world->SetContactFilter(&contactDisabler);

	// Run the simulation and verify that particles don't pass each other.
	RunStep(60.0f, 3.0f);
	// WARNING: This assumes particle indicies have not been reallocated during
	// the simulation.
	b2Vec2* positions = m_particleSystem->GetPositionBuffer();
	b2Vec2* velocities = m_particleSystem->GetVelocityBuffer();
	EXPECT_LT(positions[particleIndexA].x, positions[particleIndexB].x);
	EXPECT_LT(b2Abs(velocities[particleIndexA].x), m_particleDiameter);
	EXPECT_LT(b2Abs(velocities[particleIndexB].x), m_particleDiameter);

	// Disable particle / particle contacts.
	contactDisabler.m_enableParticleParticleCollisions = false;

	// Reset the positions and velocities of the particles.
	positions[particleIndexA].Set(-m_particleDiameter * 2.0f, 0.0f);
	velocities[particleIndexA].Set(m_particleDiameter, 0.0f);
	positions[particleIndexB].Set(m_particleDiameter * 2.0f, 0.0f);
	velocities[particleIndexB].Set(-m_particleDiameter, 0.0f);

	// Run the simulation and verify that particles now pass each other (i.e
	// they no longer collide).
	RunStep(60.0f, 3.0f);
	positions = m_particleSystem->GetPositionBuffer();
	velocities = m_particleSystem->GetVelocityBuffer();
	EXPECT_GT(positions[particleIndexA].x, positions[particleIndexB].x);
	EXPECT_FLOAT_EQ(velocities[particleIndexA].x, m_particleDiameter);
	EXPECT_FLOAT_EQ(velocities[particleIndexB].x, -m_particleDiameter);
}
bool b2DistanceJoint::SolvePositionConstraints()
{
	if (m_frequencyHz > 0.0f)
	{
		return true;
	}

	b2Body* b1 = m_body1;
	b2Body* b2 = m_body2;

	b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
	b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());

	b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;

	float32 length = d.Normalize();
	float32 C = length - m_length;
	C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);

	float32 impulse = -m_mass * C;
	m_u = d;
	b2Vec2 P = impulse * m_u;

	b1->m_sweep.c -= b1->m_invMass * P;
	b1->m_sweep.a -= b1->m_invI * b2Cross(r1, P);
	b2->m_sweep.c += b2->m_invMass * P;
	b2->m_sweep.a += b2->m_invI * b2Cross(r2, P);

	b1->SynchronizeTransform();
	b2->SynchronizeTransform();

	return b2Abs(C) < b2_linearSlop;
}
Exemple #3
0
void b2PolyShape::ResetProxy(b2BroadPhase* broadPhase)
{
	if (m_proxyId == b2_nullProxy)
	{	
		return;
	}

	b2Proxy* proxy = broadPhase->GetProxy(m_proxyId);

	broadPhase->DestroyProxy(m_proxyId);
	proxy = NULL;

	b2Mat22 R = b2Mul(m_R, m_localOBB.R);
	b2Mat22 absR = b2Abs(R);
	b2Vec2 h = b2Mul(absR, m_localOBB.extents);
	b2Vec2 position = m_position + b2Mul(m_R, m_localOBB.center);
	b2AABB aabb;
	aabb.minVertex = position - h;
	aabb.maxVertex = position + h;

	if (broadPhase->InRange(aabb))
	{
		m_proxyId = broadPhase->CreateProxy(aabb, this);
	}
	else
	{
		m_proxyId = b2_nullProxy;
	}

	if (m_proxyId == b2_nullProxy)
	{
		m_body->Freeze();
	}
}
Exemple #4
0
static bool InPoints(const b2Vec2& w, const b2Vec2* points, int32 pointCount)
{
	const float32 k_tolerance = 100.0f * B2_FLT_EPSILON;
	for (int32 i = 0; i < pointCount; ++i)
	{
		b2Vec2 d = b2Abs(w - points[i]);
		b2Vec2 m = b2Max(b2Abs(w), b2Abs(points[i]));
		
		if (d.x < k_tolerance * (m.x + 1.0f) &&
			d.y < k_tolerance * (m.y + 1.0f))
		{
			return true;
		}
	}

	return false;
}
Exemple #5
0
void b2PolyShape::Synchronize(	const b2Vec2& position1, const b2Mat22& R1,
								const b2Vec2& position2, const b2Mat22& R2)
{
	// The body transform is copied for convenience.
	m_R = R2;
	m_position = position2 + b2Mul(R2, m_localCentroid);

	if (m_proxyId == b2_nullProxy)
	{	
		return;
	}

	b2AABB aabb1, aabb2;

	{
		b2Mat22 obbR = b2Mul(R1, m_localOBB.R);
		b2Mat22 absR = b2Abs(obbR);
		b2Vec2 h = b2Mul(absR, m_localOBB.extents);
		b2Vec2 center = position1 + b2Mul(R1, m_localCentroid + m_localOBB.center);
		aabb1.minVertex = center - h;
		aabb1.maxVertex = center + h;
	}

	{
		b2Mat22 obbR = b2Mul(R2, m_localOBB.R);
		b2Mat22 absR = b2Abs(obbR);
		b2Vec2 h = b2Mul(absR, m_localOBB.extents);
		b2Vec2 center = position2 + b2Mul(R2, m_localCentroid + m_localOBB.center);
		aabb2.minVertex = center - h;
		aabb2.maxVertex = center + h;
	}

	b2AABB aabb;
	aabb.minVertex = b2Min(aabb1.minVertex, aabb2.minVertex);
	aabb.maxVertex = b2Max(aabb1.maxVertex, aabb2.maxVertex);

	b2BroadPhase* broadPhase = m_body->m_world->m_broadPhase;
	if (broadPhase->InRange(aabb))
	{
		broadPhase->MoveProxy(m_proxyId, aabb);
	}
	else
	{
		m_body->Freeze();
	}
}
Exemple #6
0
bool b2WheelJoint::SolvePositionConstraints(float32 baumgarte)
{
	B2_NOT_USED(baumgarte);

	b2Body* bA = m_bodyA;
	b2Body* bB = m_bodyB;

	b2Vec2 xA = bA->m_sweep.c;
	float32 angleA = bA->m_sweep.a;

	b2Vec2 xB = bB->m_sweep.c;
	float32 angleB = bB->m_sweep.a;

	b2Mat22 RA(angleA), RB(angleB);

	b2Vec2 rA = b2Mul(RA, m_localAnchorA - m_localCenterA);
	b2Vec2 rB = b2Mul(RB, m_localAnchorB - m_localCenterB);
	b2Vec2 d = xB + rB - xA - rA;

	b2Vec2 ay = b2Mul(RA, m_localYAxisA);

	float32 sAy = b2Cross(d + rA, ay);
	float32 sBy = b2Cross(rB, ay);

	float32 C = b2Dot(d, ay);

	float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;

	float32 impulse;
	if (k != 0.0f)
	{
		impulse = - C / k;
	}
	else
	{
		impulse = 0.0f;
	}

	b2Vec2 P = impulse * ay;
	float32 LA = impulse * sAy;
	float32 LB = impulse * sBy;

	xA -= m_invMassA * P;
	angleA -= m_invIA * LA;
	xB += m_invMassB * P;
	angleB += m_invIB * LB;

	// TODO_ERIN remove need for this.
	bA->m_sweep.c = xA;
	bA->m_sweep.a = angleA;
	bB->m_sweep.c = xB;
	bB->m_sweep.a = angleB;
	bA->SynchronizeTransform();
	bB->SynchronizeTransform();

	return b2Abs(C) <= b2_linearSlop;
}
Exemple #7
0
void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2XForm& xf) const
{
	b2Mat22 R = b2Mul(xf.R, m_obb.R);
	b2Mat22 absR = b2Abs(R);
	b2Vec2 h = b2Mul(absR, m_obb.extents);
	b2Vec2 position = xf.position + b2Mul(xf.R, m_obb.center);
	aabb->lowerBound = position - h;
	aabb->upperBound = position + h;
}
bool b2WeldJoint::SolvePositionConstraints(float32 baumgarte)
{
	B2_NOT_USED(baumgarte);

	b2Body* bA = m_bodyA;
	b2Body* bB = m_bodyB;

	float32 mA = bA->m_invMass, mB = bB->m_invMass;
	float32 iA = bA->m_invI, iB = bB->m_invI;

	b2Vec2 rA = b2Mul(bA->GetTransform().R, m_localAnchorA - bA->GetLocalCenter());
	b2Vec2 rB = b2Mul(bB->GetTransform().R, m_localAnchorB - bB->GetLocalCenter());

	b2Vec2 C1 =  bB->m_sweep.c + rB - bA->m_sweep.c - rA;
	float32 C2 = bB->m_sweep.a - bA->m_sweep.a - m_referenceAngle;

	// Handle large detachment.
	const float32 k_allowedStretch = 10.0f * b2_linearSlop;
	float32 positionError = C1.Length();
	float32 angularError = b2Abs(C2);
	if (positionError > k_allowedStretch)
	{
		iA *= 1.0f;
		iB *= 1.0f;
	}

	m_mass.col1.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
	m_mass.col2.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
	m_mass.col3.x = -rA.y * iA - rB.y * iB;
	m_mass.col1.y = m_mass.col2.x;
	m_mass.col2.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
	m_mass.col3.y = rA.x * iA + rB.x * iB;
	m_mass.col1.z = m_mass.col3.x;
	m_mass.col2.z = m_mass.col3.y;
	m_mass.col3.z = iA + iB;

	b2Vec3 C(C1.x, C1.y, C2);

	b2Vec3 impulse = m_mass.Solve33(-C);

	b2Vec2 P(impulse.x, impulse.y);

	bA->m_sweep.c -= mA * P;
	bA->m_sweep.a -= iA * (b2Cross(rA, P) + impulse.z);

	bB->m_sweep.c += mB * P;
	bB->m_sweep.a += iB * (b2Cross(rB, P) + impulse.z);

	bA->SynchronizeTransform();
	bB->SynchronizeTransform();

	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
bool b2FixedJoint::SolvePositionConstraints(float32 baumgarte)
{
	B2_NOT_USED(baumgarte);

	// Get bodies
	b2Body* b1 = m_body1;
	b2Body* b2 = m_body2;

	// Recalculate effective constraint mass if angle changed enough
	if (b2Abs(b1->m_sweep.a - m_a1) > 1e-3f)
		CalculateMC();

	// Calculate C
	float C[3] =
	{
		b2->m_sweep.c.x - b1->m_sweep.c.x - m_c * m_d.x + m_s * m_d.y,
		b2->m_sweep.c.y - b1->m_sweep.c.y - m_s * m_d.x - m_c * m_d.y,
		b2->m_sweep.a - m_a1 - m_a
	};

	// Calculate lambda
	float lambda[3];
	for (int r = 0; r < 3; ++r)
		lambda[r] = -(m_mc[r][0] * C[0] + m_mc[r][1] * C[1] + m_mc[r][2] * C[2]);

	// Apply impulse
	b1->m_sweep.c.x -= b1->m_invMass * lambda[0];
	b1->m_sweep.c.y -= b1->m_invMass * lambda[1];
	b1->m_sweep.a -= b1->m_invI * (lambda[0]*m_Ax+lambda[1]*m_Ay+lambda[2]);
	b2->m_sweep.c.x += b2->m_invMass * lambda[0];
	b2->m_sweep.c.y += b2->m_invMass * lambda[1];
	b2->m_sweep.a += b2->m_invI * lambda[2];

	// Push the changes to the transforms
	b1->SynchronizeTransform();
	b2->SynchronizeTransform();

	// Constraint is satisfied if all constraint equations are nearly zero
	return b2Abs(C[0]) < b2_linearSlop && b2Abs(C[1]) < b2_linearSlop && b2Abs(C[2]) < b2_angularSlop;
}
Exemple #10
0
static float32 calcDistanceToLine( const Vec2& pt,
				 const Vec2& l1, const Vec2& l2,
				 bool* withinLine=NULL )
{
  b2Vec2 l = l2 - l1; 
  b2Vec2 w = pt - l1;
  float32 mag = l.Normalize();
  float32 dist = b2Cross( w, l );
  if ( withinLine ) {
    float32 dot = b2Dot( l, w );
    *withinLine = ( dot >= 0.0f && dot <= mag );
  }  
  return b2Abs( dist );
}
bool b2WheelJoint::SolvePositionConstraints(const b2SolverData& data)
{
	b2Vec2 cA = data.positions[m_indexA].c;
	float32 aA = data.positions[m_indexA].a;
	b2Vec2 cB = data.positions[m_indexB].c;
	float32 aB = data.positions[m_indexB].a;

	b2Rot qA(aA), qB(aB);

	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
	b2Vec2 d = (cB - cA) + rB - rA;

	b2Vec2 ay = b2Mul(qA, m_localYAxisA);

	float32 sAy = b2Cross(d + rA, ay);
	float32 sBy = b2Cross(rB, ay);

	float32 C = b2Dot(d, ay);

	float32 k = m_invMassA + m_invMassB + m_invIA * m_sAy * m_sAy + m_invIB * m_sBy * m_sBy;

	float32 impulse;
	if (k != 0.0f)
	{
		impulse = - C / k;
	}
	else
	{
		impulse = 0.0f;
	}

	b2Vec2 P = impulse * ay;
	float32 LA = impulse * sAy;
	float32 LB = impulse * sBy;

	cA -= m_invMassA * P;
	aA -= m_invIA * LA;
	cB += m_invMassB * P;
	aB += m_invIB * LB;

	data.positions[m_indexA].c = cA;
	data.positions[m_indexA].a = aA;
	data.positions[m_indexB].c = cB;
	data.positions[m_indexB].a = aB;

	return b2Abs(C) <= b2_linearSlop;
}
bool b2ElasticRopeJoint::SolvePositionConstraints(const b2SolverData& data)
{
    if (m_frequencyHz > 0.0f)
    {
        // There is no position correction for soft distance constraints.
        return true;
    }

    b2Vec2 cA = data.positions[m_indexA].c;
    float32 aA = data.positions[m_indexA].a;
    b2Vec2 cB = data.positions[m_indexB].c;
    float32 aB = data.positions[m_indexB].a;

    b2Rot qA(aA), qB(aB);

    b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
    b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
    b2Vec2 u = cB + rB - cA - rA;

    float32 length = u.Normalize();

    float32 C;
    if(length-m_length < 0) {
        C = 0;//length - m_length;
    } else {
        C = length - m_length;
    }


    C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);

    float32 impulse = -m_mass * C;
    b2Vec2 P = impulse * u;

    cA -= m_invMassA * P;
    aA -= m_invIA * b2Cross(rA, P);
    cB += m_invMassB * P;
    aB += m_invIB * b2Cross(rB, P);

    data.positions[m_indexA].c = cA;
    data.positions[m_indexA].a = aA;
    data.positions[m_indexB].c = cB;
    data.positions[m_indexB].a = aB;

    return b2Abs(C) < b2_linearSlop;
}
int32 b2DynamicTree::GetMaxBalance() const
{
	int32 maxBalance = 0;
	for (int32 i = 0; i < m_nodeCapacity; ++i)
	{
		const b2TreeNode* node = m_nodes + i;
		if (node->height <= 1)
		{
			continue;
		}

		b2Assert(node->IsLeaf() == false);

		int32 child1 = node->child1;
		int32 child2 = node->child2;
		int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
		maxBalance = b2Max(maxBalance, balance);
	}

	return maxBalance;
}
bool b2DistanceJoint::SolvePositionConstraints(float32 baumgarte)
{
    B2_NOT_USED(baumgarte);

    if (m_frequencyHz > 0.0f)
    {
        // There is no position correction for soft distance constraints.
        return true;
    }

    b2Body* b1 = m_body1;
    b2Body* b2 = m_body2;

    b2Vec2 r1 = b2Mul(b1->GetXForm().R, m_localAnchor1 - b1->GetLocalCenter());
    b2Vec2 r2 = b2Mul(b2->GetXForm().R, m_localAnchor2 - b2->GetLocalCenter());

    b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;

    float32 length = d.Normalize();
    float32 C = length - m_length;
    C = b2Clamp(C, -(settings->b2_maxLinearCorrection), settings->b2_maxLinearCorrection);

    float32 impulse = -m_mass * C;
    m_u = d;
    b2Vec2 P = impulse * m_u;

    b1->m_sweep.c -= b1->m_invMass * P;
    b1->m_sweep.a -= b1->m_invI * b2Cross(r1, P);
    b2->m_sweep.c += b2->m_invMass * P;
    b2->m_sweep.a += b2->m_invI * b2Cross(r2, P);

    b1->SynchronizeTransform();
    b2->SynchronizeTransform();

    return b2Abs(C) < settings->b2_linearSlop;
}
Exemple #15
0
void b2LineJoint::InitVelocityConstraints(const b2TimeStep& step)
{
	b2Body* b1 = m_bodyA;
	b2Body* b2 = m_bodyB;

	m_localCenterA = b1->GetLocalCenter();
	m_localCenterB = b2->GetLocalCenter();

	b2Transform xf1 = b1->GetTransform();
	b2Transform xf2 = b2->GetTransform();

	// Compute the effective masses.
	b2Vec2 r1 = b2Mul(xf1.R, m_localAnchor1 - m_localCenterA);
	b2Vec2 r2 = b2Mul(xf2.R, m_localAnchor2 - m_localCenterB);
	b2Vec2 d = b2->m_sweep.c + r2 - b1->m_sweep.c - r1;

	m_invMassA = b1->m_invMass;
	m_invIA = b1->m_invI;
	m_invMassB = b2->m_invMass;
	m_invIB = b2->m_invI;

	// Compute motor Jacobian and effective mass.
	{
		m_axis = b2Mul(xf1.R, m_localXAxis1);
		m_a1 = b2Cross(d + r1, m_axis);
		m_a2 = b2Cross(r2, m_axis);

		m_motorMass = m_invMassA + m_invMassB + m_invIA * m_a1 * m_a1 + m_invIB * m_a2 * m_a2;
		if (m_motorMass > b2_epsilon)
		{
			m_motorMass = 1.0f / m_motorMass;
		}
		else
		{
			m_motorMass = 0.0f;
		}
	}

	// Prismatic constraint.
	{
		m_perp = b2Mul(xf1.R, m_localYAxis1);

		m_s1 = b2Cross(d + r1, m_perp);
		m_s2 = b2Cross(r2, m_perp);

		float32 m1 = m_invMassA, m2 = m_invMassB;
		float32 i1 = m_invIA, i2 = m_invIB;

		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
		float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
		float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;

		m_K.col1.Set(k11, k12);
		m_K.col2.Set(k12, k22);
	}

	// Compute motor and limit terms.
	if (m_enableLimit)
	{
		float32 jointTranslation = b2Dot(m_axis, d);
		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
		{
			m_limitState = e_equalLimits;
		}
		else if (jointTranslation <= m_lowerTranslation)
		{
			if (m_limitState != e_atLowerLimit)
			{
				m_limitState = e_atLowerLimit;
				m_impulse.y = 0.0f;
			}
		}
		else if (jointTranslation >= m_upperTranslation)
		{
			if (m_limitState != e_atUpperLimit)
			{
				m_limitState = e_atUpperLimit;
				m_impulse.y = 0.0f;
			}
		}
		else
		{
			m_limitState = e_inactiveLimit;
			m_impulse.y = 0.0f;
		}
	}
	else
	{
		m_limitState = e_inactiveLimit;
	}

	if (m_enableMotor == false)
	{
		m_motorImpulse = 0.0f;
	}

	if (step.warmStarting)
	{
		// Account for variable time step.
		m_impulse *= step.dtRatio;
		m_motorImpulse *= step.dtRatio;

		b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.y) * m_axis;
		float32 L1 = m_impulse.x * m_s1 + (m_motorImpulse + m_impulse.y) * m_a1;
		float32 L2 = m_impulse.x * m_s2 + (m_motorImpulse + m_impulse.y) * m_a2;

		b1->m_linearVelocity -= m_invMassA * P;
		b1->m_angularVelocity -= m_invIA * L1;

		b2->m_linearVelocity += m_invMassB * P;
		b2->m_angularVelocity += m_invIB * L2;
	}
	else
	{
		m_impulse.SetZero();
		m_motorImpulse = 0.0f;
	}
}
void b2RevoluteJoint::InitVelocityConstraints(const b2SolverData& data)
{
	m_indexA = m_bodyA->m_islandIndex;
	m_indexB = m_bodyB->m_islandIndex;
	m_localCenterA = m_bodyA->m_sweep.localCenter;
	m_localCenterB = m_bodyB->m_sweep.localCenter;
	m_invMassA = m_bodyA->m_invMass;
	m_invMassB = m_bodyB->m_invMass;
	m_invIA = m_bodyA->m_invI;
	m_invIB = m_bodyB->m_invI;

	b2Vec2 cA = data.positions[m_indexA].c;
	float32 aA = data.positions[m_indexA].a;
	b2Vec2 vA = data.velocities[m_indexA].v;
	float32 wA = data.velocities[m_indexA].w;

	b2Vec2 cB = data.positions[m_indexB].c;
	float32 aB = data.positions[m_indexB].a;
	b2Vec2 vB = data.velocities[m_indexB].v;
	float32 wB = data.velocities[m_indexB].w;

	b2Rot qA(aA), qB(aB);

	m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
	m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);

	// J = [-I -r1_skew I r2_skew]
	//     [ 0       -1 0       1]
	// r_skew = [-ry; rx]

	// Matlab
	// K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB]
	//     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB]
	//     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB]

	float32 mA = m_invMassA, mB = m_invMassB;
	float32 iA = m_invIA, iB = m_invIB;

	bool fixedRotation = (iA + iB == 0.0f);

	m_mass.ex.x = mA + mB + m_rA.y * m_rA.y * iA + m_rB.y * m_rB.y * iB;
	m_mass.ey.x = -m_rA.y * m_rA.x * iA - m_rB.y * m_rB.x * iB;
	m_mass.ez.x = -m_rA.y * iA - m_rB.y * iB;
	m_mass.ex.y = m_mass.ey.x;
	m_mass.ey.y = mA + mB + m_rA.x * m_rA.x * iA + m_rB.x * m_rB.x * iB;
	m_mass.ez.y = m_rA.x * iA + m_rB.x * iB;
	m_mass.ex.z = m_mass.ez.x;
	m_mass.ey.z = m_mass.ez.y;
	m_mass.ez.z = iA + iB;

	m_motorMass = iA + iB;
	if (m_motorMass > 0.0f)
	{
		m_motorMass = 1.0f / m_motorMass;
	}

	if (m_enableMotor == false || fixedRotation)
	{
		m_motorImpulse = 0.0f;
	}

	if (m_enableLimit && fixedRotation == false)
	{
		float32 jointAngle = aB - aA - m_referenceAngle;
		if (b2Abs(m_upperAngle - m_lowerAngle) < 2.0f * b2_angularSlop)
		{
			m_limitState = e_equalLimits;
		}
		else if (jointAngle <= m_lowerAngle)
		{
			if (m_limitState != e_atLowerLimit)
			{
				m_impulse.z = 0.0f;
			}
			m_limitState = e_atLowerLimit;
		}
		else if (jointAngle >= m_upperAngle)
		{
			if (m_limitState != e_atUpperLimit)
			{
				m_impulse.z = 0.0f;
			}
			m_limitState = e_atUpperLimit;
		}
		else
		{
			m_limitState = e_inactiveLimit;
			m_impulse.z = 0.0f;
		}
	}
	else
	{
		m_limitState = e_inactiveLimit;
	}

	if (data.step.warmStarting)
	{
		// Scale impulses to support a variable time step.
		m_impulse *= data.step.dtRatio;
		m_motorImpulse *= data.step.dtRatio;

		b2Vec2 P(m_impulse.x, m_impulse.y);

		vA -= mA * P;
		wA -= iA * (b2Cross(m_rA, P) + m_motorImpulse + m_impulse.z);

		vB += mB * P;
		wB += iB * (b2Cross(m_rB, P) + m_motorImpulse + m_impulse.z);
	}
	else
	{
		m_impulse.SetZero();
		m_motorImpulse = 0.0f;
	}

	data.velocities[m_indexA].v = vA;
	data.velocities[m_indexA].w = wA;
	data.velocities[m_indexB].v = vB;
	data.velocities[m_indexB].w = wB;
}
bool b2RevoluteJoint::SolvePositionConstraints(const b2SolverData& data)
{
	b2Vec2 cA = data.positions[m_indexA].c;
	float32 aA = data.positions[m_indexA].a;
	b2Vec2 cB = data.positions[m_indexB].c;
	float32 aB = data.positions[m_indexB].a;

	b2Rot qA(aA), qB(aB);

	float32 angularError = 0.0f;
	float32 positionError = 0.0f;

	bool fixedRotation = (m_invIA + m_invIB == 0.0f);

	// Solve angular limit constraint.
	if (m_enableLimit && m_limitState != e_inactiveLimit && fixedRotation == false)
	{
		float32 angle = aB - aA - m_referenceAngle;
		float32 limitImpulse = 0.0f;

		if (m_limitState == e_equalLimits)
		{
			// Prevent large angular corrections
			float32 C = b2Clamp(angle - m_lowerAngle, -b2_maxAngularCorrection, b2_maxAngularCorrection);
			limitImpulse = -m_motorMass * C;
			angularError = b2Abs(C);
		}
		else if (m_limitState == e_atLowerLimit)
		{
			float32 C = angle - m_lowerAngle;
			angularError = -C;

			// Prevent large angular corrections and allow some slop.
			C = b2Clamp(C + b2_angularSlop, -b2_maxAngularCorrection, 0.0f);
			limitImpulse = -m_motorMass * C;
		}
		else if (m_limitState == e_atUpperLimit)
		{
			float32 C = angle - m_upperAngle;
			angularError = C;

			// Prevent large angular corrections and allow some slop.
			C = b2Clamp(C - b2_angularSlop, 0.0f, b2_maxAngularCorrection);
			limitImpulse = -m_motorMass * C;
		}

		aA -= m_invIA * limitImpulse;
		aB += m_invIB * limitImpulse;
	}

	// Solve point-to-point constraint.
	{
		qA.Set(aA);
		qB.Set(aB);
		b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
		b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);

		b2Vec2 C = cB + rB - cA - rA;
		positionError = C.Length();

		float32 mA = m_invMassA, mB = m_invMassB;
		float32 iA = m_invIA, iB = m_invIB;

		b2Mat22 K;
		K.ex.x = mA + mB + iA * rA.y * rA.y + iB * rB.y * rB.y;
		K.ex.y = -iA * rA.x * rA.y - iB * rB.x * rB.y;
		K.ey.x = K.ex.y;
		K.ey.y = mA + mB + iA * rA.x * rA.x + iB * rB.x * rB.x;

		b2Vec2 impulse = -K.Solve(C);

		cA -= mA * impulse;
		aA -= iA * b2Cross(rA, impulse);

		cB += mB * impulse;
		aB += iB * b2Cross(rB, impulse);
	}

	data.positions[m_indexA].c = cA;
	data.positions[m_indexA].a = aA;
	data.positions[m_indexB].c = cB;
	data.positions[m_indexB].a = aB;

	return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
Exemple #18
0
float32 b2TimeOfImpact(const b2TOIInput* input, const TA* shapeA, const TB* shapeB)
{
	b2Sweep sweepA = input->sweepA;
	b2Sweep sweepB = input->sweepB;

	float32 r1 = input->sweepRadiusA;
	float32 r2 = input->sweepRadiusB;

	float32 tolerance = input->tolerance;

	float32 radius = shapeA->m_radius + shapeB->m_radius;

	b2Assert(sweepA.t0 == sweepB.t0);
	b2Assert(1.0f - sweepA.t0 > B2_FLT_EPSILON);

	b2Vec2 v1 = sweepA.c - sweepA.c0;
	b2Vec2 v2 = sweepB.c - sweepB.c0;
	float32 omega1 = sweepA.a - sweepA.a0;
	float32 omega2 = sweepB.a - sweepB.a0;

	float32 alpha = 0.0f;

	b2DistanceInput distanceInput;
	distanceInput.useRadii = false;
	b2SimplexCache cache;
	cache.count = 0;

	b2Vec2 p1, p2;
	const int32 k_maxIterations = 1000;	// TODO_ERIN b2Settings
	int32 iter = 0;
	b2Vec2 normal = b2Vec2_zero;
	float32 distance = 0.0f;
	float32 targetDistance = 0.0f;
	for(;;)
	{
		b2XForm xf1, xf2;
		sweepA.GetTransform(&xf1, alpha);
		sweepB.GetTransform(&xf2, alpha);

		// Get the distance between shapes.
		distanceInput.transformA = xf1;
		distanceInput.transformB = xf2;
		b2DistanceOutput distanceOutput;
		b2Distance(&distanceOutput, &cache, &distanceInput, shapeA, shapeB);
		distance = distanceOutput.distance;
		p1 = distanceOutput.pointA;
		p2 = distanceOutput.pointB;

		if (iter == 0)
		{
			// Compute a reasonable target distance to give some breathing room
			// for conservative advancement.
			if (distance > radius)
			{
				targetDistance = b2Max(radius - tolerance, 0.75f * radius);
			}
			else
			{
				targetDistance = b2Max(distance - tolerance, 0.02f * radius);
			}
		}

		if (distance - targetDistance < 0.5f * tolerance || iter == k_maxIterations)
		{
			break;
		}

		normal = p2 - p1;
		normal.Normalize();

		// Compute upper bound on remaining movement.
		float32 approachVelocityBound = b2Dot(normal, v1 - v2) + b2Abs(omega1) * r1 + b2Abs(omega2) * r2;
		if (b2Abs(approachVelocityBound) < B2_FLT_EPSILON)
		{
			alpha = 1.0f;
			break;
		}

		// Get the conservative time increment. Don't advance all the way.
		float32 dAlpha = (distance - targetDistance) / approachVelocityBound;
		//float32 dt = (distance - 0.5f * b2_linearSlop) / approachVelocityBound;
		float32 newAlpha = alpha + dAlpha;

		// The shapes may be moving apart or a safe distance apart.
		if (newAlpha < 0.0f || 1.0f < newAlpha)
		{
			alpha = 1.0f;
			break;
		}

		// Ensure significant advancement.
		if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha)
		{
			break;
		}

		alpha = newAlpha;

		++iter;
	}

	b2_maxToiIters = b2Max(iter, b2_maxToiIters);

	return alpha;
}
Exemple #19
0
float32 b2TimeOfImpact(const b2TOIInput* input, const TA* shapeA, const TB* shapeB)
{
	b2Sweep sweepA = input->sweepA;
	b2Sweep sweepB = input->sweepB;

	b2Assert(sweepA.t0 == sweepB.t0);
	b2Assert(1.0f - sweepA.t0 > B2_FLT_EPSILON);

	float32 radius = shapeA->m_radius + shapeB->m_radius;
	float32 tolerance = input->tolerance;

	float32 alpha = 0.0f;

	const int32 k_maxIterations = 1000;	// TODO_ERIN b2Settings
	int32 iter = 0;
	float32 target = 0.0f;

	// Prepare input for distance query.
	b2SimplexCache cache;
	cache.count = 0;
	b2DistanceInput distanceInput;
	distanceInput.useRadii = false;

	for(;;)
	{
		b2XForm xfA, xfB;
		sweepA.GetTransform(&xfA, alpha);
		sweepB.GetTransform(&xfB, alpha);

		// Get the distance between shapes.
		distanceInput.transformA = xfA;
		distanceInput.transformB = xfB;
		b2DistanceOutput distanceOutput;
		b2Distance(&distanceOutput, &cache, &distanceInput, shapeA, shapeB);

		if (distanceOutput.distance <= 0.0f)
		{
			alpha = 1.0f;
			break;
		}

		b2SeparationFunction<TA, TB> fcn;
		fcn.Initialize(&cache, shapeA, xfA, shapeB, xfB);

		float32 separation = fcn.Evaluate(xfA, xfB);
		if (separation <= 0.0f)
		{
			alpha = 1.0f;
			break;
		}

		if (iter == 0)
		{
			// Compute a reasonable target distance to give some breathing room
			// for conservative advancement. We take advantage of the shape radii
			// to create additional clearance.
			if (separation > radius)
			{
				target = b2Max(radius - tolerance, 0.75f * radius);
			}
			else
			{
				target = b2Max(separation - tolerance, 0.02f * radius);
			}
		}

		if (separation - target < 0.5f * tolerance)
		{
			if (iter == 0)
			{
				alpha = 1.0f;
				break;
			}

			break;
		}

#if 0
		// Dump the curve seen by the root finder
		{
			const int32 N = 100;
			float32 dx = 1.0f / N;
			float32 xs[N+1];
			float32 fs[N+1];

			float32 x = 0.0f;

			for (int32 i = 0; i <= N; ++i)
			{
				sweepA.GetTransform(&xfA, x);
				sweepB.GetTransform(&xfB, x);
				float32 f = fcn.Evaluate(xfA, xfB) - target;

				printf("%g %g\n", x, f);

				xs[i] = x;
				fs[i] = f;

				x += dx;
			}
		}
#endif

		// Compute 1D root of: f(x) - target = 0
		float32 newAlpha = alpha;
		{
			float32 x1 = alpha, x2 = 1.0f;

			float32 f1 = separation;

			sweepA.GetTransform(&xfA, x2);
			sweepB.GetTransform(&xfB, x2);
			float32 f2 = fcn.Evaluate(xfA, xfB);

			// If intervals don't overlap at t2, then we are done.
			if (f2 >= target)
			{
				alpha = 1.0f;
				break;
			}

			// Determine when intervals intersect.
			int32 rootIterCount = 0;
			for (;;)
			{
				// Use a mix of the secant rule and bisection.
				float32 x;
				if (rootIterCount & 1)
				{
					// Secant rule to improve convergence.
					x = x1 + (target - f1) * (x2 - x1) / (f2 - f1);
				}
				else
				{
					// Bisection to guarantee progress.
					x = 0.5f * (x1 + x2);
				}

				sweepA.GetTransform(&xfA, x);
				sweepB.GetTransform(&xfB, x);

				float32 f = fcn.Evaluate(xfA, xfB);

				if (b2Abs(f - target) < 0.025f * tolerance)
				{
					newAlpha = x;
					break;
				}

				// Ensure we continue to bracket the root.
				if (f > target)
				{
					x1 = x;
					f1 = f;
				}
				else
				{
					x2 = x;
					f2 = f;
				}

				++rootIterCount;

				//b2Assert(rootIterCount < 50);
				if (rootIterCount >= 50 )
				{
					break;
				}
			}

			b2_maxToiRootIters = b2Max(b2_maxToiRootIters, rootIterCount);
		}

		// Ensure significant advancement.
		if (newAlpha < (1.0f + 100.0f * B2_FLT_EPSILON) * alpha)
		{
			break;
		}

		alpha = newAlpha;

		++iter;

		if (iter == k_maxIterations)
		{
			break;
		}
	}

	b2_maxToiIters = b2Max(b2_maxToiIters, iter);

	return alpha;
}
bool b2PulleyJoint::SolvePositionConstraints(const b2SolverData& data)
{
    b2Vec2 cA = data.positions[m_indexA].c;
    float32 aA = data.positions[m_indexA].a;
    b2Vec2 cB = data.positions[m_indexB].c;
    float32 aB = data.positions[m_indexB].a;

    b2Rot qA(aA), qB(aB);

    b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
    b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);

    // Get the pulley axes.
    b2Vec2 uA = cA + rA - m_groundAnchorA;
    b2Vec2 uB = cB + rB - m_groundAnchorB;

    float32 lengthA = uA.Length();
    float32 lengthB = uB.Length();

    if (lengthA > 10.0f * b2_linearSlop)
    {
        uA *= 1.0f / lengthA;
    }
    else
    {
        uA.SetZero();
    }

    if (lengthB > 10.0f * b2_linearSlop)
    {
        uB *= 1.0f / lengthB;
    }
    else
    {
        uB.SetZero();
    }

    // Compute effective mass.
    float32 ruA = b2Cross(rA, uA);
    float32 ruB = b2Cross(rB, uB);

    float32 mA = m_invMassA + m_invIA * ruA * ruA;
    float32 mB = m_invMassB + m_invIB * ruB * ruB;

    float32 mass = mA + m_ratio * m_ratio * mB;

    if (mass > 0.0f)
    {
        mass = 1.0f / mass;
    }

    float32 C = m_constant - lengthA - m_ratio * lengthB;
    float32 linearError = b2Abs(C);

    float32 impulse = -mass * C;

    b2Vec2 PA = -impulse * uA;
    b2Vec2 PB = -m_ratio * impulse * uB;

    cA += m_invMassA * PA;
    aA += m_invIA * b2Cross(rA, PA);
    cB += m_invMassB * PB;
    aB += m_invIB * b2Cross(rB, PB);

    data.positions[m_indexA].c = cA;
    data.positions[m_indexA].a = aA;
    data.positions[m_indexB].c = cB;
    data.positions[m_indexB].a = aB;

    return linearError < b2_linearSlop;
}
void b2ContactSolver::SolveVelocityConstraints()
{
	for (int32 i = 0; i < m_count; ++i)
	{
		b2ContactVelocityConstraint* vc = m_velocityConstraints + i;

		int32 indexA = vc->indexA;
		int32 indexB = vc->indexB;
		float32 mA = vc->invMassA;
		float32 iA = vc->invIA;
		float32 mB = vc->invMassB;
		float32 iB = vc->invIB;
		int32 pointCount = vc->pointCount;

		b2Vec2 vA = m_velocities[indexA].v;
		float32 wA = m_velocities[indexA].w;
		b2Vec2 vB = m_velocities[indexB].v;
		float32 wB = m_velocities[indexB].w;

		b2Vec2 normal = vc->normal;
		b2Vec2 tangent = b2Cross(normal, 1.0f);
		float32 friction = vc->friction;

		b2Assert(pointCount == 1 || pointCount == 2);

		// Solve tangent constraints first because non-penetration is more important
		// than friction.
		for (int32 j = 0; j < pointCount; ++j)
		{
			b2VelocityConstraintPoint* vcp = vc->points + j;

			// Relative velocity at contact
			b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);

			// Compute tangent force
			float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;
			float32 lambda = vcp->tangentMass * (-vt);

			// b2Clamp the accumulated force
			float32 maxFriction = friction * vcp->normalImpulse;
			float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
			lambda = newImpulse - vcp->tangentImpulse;
			vcp->tangentImpulse = newImpulse;

			// Apply contact impulse
			b2Vec2 P = lambda * tangent;

			vA -= mA * P;
			wA -= iA * b2Cross(vcp->rA, P);

			vB += mB * P;
			wB += iB * b2Cross(vcp->rB, P);
		}

		// Solve normal constraints
		if (pointCount == 1 || g_blockSolve == false)
		{
			for (int32 j = 0; j < pointCount; ++j)
			{
				b2VelocityConstraintPoint* vcp = vc->points + j;

				// Relative velocity at contact
				b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);

				// Compute normal impulse
				float32 vn = b2Dot(dv, normal);
				float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);

				// b2Clamp the accumulated impulse
				float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
				lambda = newImpulse - vcp->normalImpulse;
				vcp->normalImpulse = newImpulse;

				// Apply contact impulse
				b2Vec2 P = lambda * normal;
				vA -= mA * P;
				wA -= iA * b2Cross(vcp->rA, P);

				vB += mB * P;
				wB += iB * b2Cross(vcp->rB, P);
			}
		}
		else
		{
			// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
			// Build the mini LCP for this contact patch
			//
			// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
			//
			// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
			// b = vn0 - velocityBias
			//
			// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
			// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
			// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
			// solution that satisfies the problem is chosen.
			// 
			// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
			// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
			//
			// Substitute:
			// 
			// x = a + d
			// 
			// a := old total impulse
			// x := new total impulse
			// d := incremental impulse 
			//
			// For the current iteration we extend the formula for the incremental impulse
			// to compute the new total impulse:
			//
			// vn = A * d + b
			//    = A * (x - a) + b
			//    = A * x + b - A * a
			//    = A * x + b'
			// b' = b - A * a;

			b2VelocityConstraintPoint* cp1 = vc->points + 0;
			b2VelocityConstraintPoint* cp2 = vc->points + 1;

			b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
			b2Assert(a.x >= 0.0f && a.y >= 0.0f);

			// Relative velocity at contact
			b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
			b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);

			// Compute normal velocity
			float32 vn1 = b2Dot(dv1, normal);
			float32 vn2 = b2Dot(dv2, normal);

			b2Vec2 b;
			b.x = vn1 - cp1->velocityBias;
			b.y = vn2 - cp2->velocityBias;

			// Compute b'
			b -= b2Mul(vc->K, a);

			const float32 k_errorTol = 1e-3f;
			B2_NOT_USED(k_errorTol);

			for (;;)
			{
				//
				// Case 1: vn = 0
				//
				// 0 = A * x + b'
				//
				// Solve for x:
				//
				// x = - inv(A) * b'
				//
				b2Vec2 x = - b2Mul(vc->normalMass, b);

				if (x.x >= 0.0f && x.y >= 0.0f)
				{
					// Get the incremental impulse
					b2Vec2 d = x - a;

					// Apply incremental impulse
					b2Vec2 P1 = d.x * normal;
					b2Vec2 P2 = d.y * normal;
					vA -= mA * (P1 + P2);
					wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));

					vB += mB * (P1 + P2);
					wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));

					// Accumulate
					cp1->normalImpulse = x.x;
					cp2->normalImpulse = x.y;

#if B2_DEBUG_SOLVER == 1
					// Postconditions
					dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
					dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);

					// Compute normal velocity
					vn1 = b2Dot(dv1, normal);
					vn2 = b2Dot(dv2, normal);

					b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
					b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
					break;
				}

				//
				// Case 2: vn1 = 0 and x2 = 0
				//
				//   0 = a11 * x1 + a12 * 0 + b1' 
				// vn2 = a21 * x1 + a22 * 0 + b2'
				//
				x.x = - cp1->normalMass * b.x;
				x.y = 0.0f;
#if B2_DEBUG_SOLVER == 1
				vn1 = 0.0f;
				vn2 = vc->K.ex.y * x.x + b.y;
#endif
				if (x.x >= 0.0f && vn2 >= 0.0f)
				{
					// Get the incremental impulse
					b2Vec2 d = x - a;

					// Apply incremental impulse
					b2Vec2 P1 = d.x * normal;
					b2Vec2 P2 = d.y * normal;
					vA -= mA * (P1 + P2);
					wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));

					vB += mB * (P1 + P2);
					wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));

					// Accumulate
					cp1->normalImpulse = x.x;
					cp2->normalImpulse = x.y;

#if B2_DEBUG_SOLVER == 1
					// Postconditions
					dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);

					// Compute normal velocity
					vn1 = b2Dot(dv1, normal);

					b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
#endif
					break;
				}


				//
				// Case 3: vn2 = 0 and x1 = 0
				//
				// vn1 = a11 * 0 + a12 * x2 + b1' 
				//   0 = a21 * 0 + a22 * x2 + b2'
				//
				x.x = 0.0f;
				x.y = - cp2->normalMass * b.y;
#if B2_DEBUG_SOLVER == 1
				vn1 = vc->K.ey.x * x.y + b.x;
				vn2 = 0.0f;
#endif

				if (x.y >= 0.0f && vn1 >= 0.0f)
				{
					// Resubstitute for the incremental impulse
					b2Vec2 d = x - a;

					// Apply incremental impulse
					b2Vec2 P1 = d.x * normal;
					b2Vec2 P2 = d.y * normal;
					vA -= mA * (P1 + P2);
					wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));

					vB += mB * (P1 + P2);
					wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));

					// Accumulate
					cp1->normalImpulse = x.x;
					cp2->normalImpulse = x.y;

#if B2_DEBUG_SOLVER == 1
					// Postconditions
					dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);

					// Compute normal velocity
					vn2 = b2Dot(dv2, normal);

					b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
					break;
				}

				//
				// Case 4: x1 = 0 and x2 = 0
				// 
				// vn1 = b1
				// vn2 = b2;
				x.x = 0.0f;
				x.y = 0.0f;
				vn1 = b.x;
				vn2 = b.y;

				if (vn1 >= 0.0f && vn2 >= 0.0f )
				{
					// Resubstitute for the incremental impulse
					b2Vec2 d = x - a;

					// Apply incremental impulse
					b2Vec2 P1 = d.x * normal;
					b2Vec2 P2 = d.y * normal;
					vA -= mA * (P1 + P2);
					wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));

					vB += mB * (P1 + P2);
					wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));

					// Accumulate
					cp1->normalImpulse = x.x;
					cp2->normalImpulse = x.y;

					break;
				}

				// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
				break;
			}
		}

		m_velocities[indexA].v = vA;
		m_velocities[indexA].w = wA;
		m_velocities[indexB].v = vB;
		m_velocities[indexB].w = wB;
	}
}
void b2Island::Solve(const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
{
	// Integrate velocities and apply damping.
	for (int32 i = 0; i < m_bodyCount; ++i)
	{
		b2Body* b = m_bodies[i];

		if (b->GetType() != b2_dynamicBody)
		{
			continue;
		}

		// Integrate velocities.
		b->m_linearVelocity += step.dt * (gravity + b->m_invMass * b->m_force);
		b->m_angularVelocity += step.dt * b->m_invI * b->m_torque;

		// Apply damping.
		// ODE: dv/dt + c * v = 0
		// Solution: v(t) = v0 * exp(-c * t)
		// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
		// v2 = exp(-c * dt) * v1
		// Taylor expansion:
		// v2 = (1.0f - c * dt) * v1
		b->m_linearVelocity *= b2Clamp(1.0f - step.dt * b->m_linearDamping, 0.0f, 1.0f);
		b->m_angularVelocity *= b2Clamp(1.0f - step.dt * b->m_angularDamping, 0.0f, 1.0f);
	}

	// Partition contacts so that contacts with static bodies are solved last.
	int32 i1 = -1;
	for (int32 i2 = 0; i2 < m_contactCount; ++i2)
	{
		b2Fixture* fixtureA = m_contacts[i2]->GetFixtureA();
		b2Fixture* fixtureB = m_contacts[i2]->GetFixtureB();
		b2Body* bodyA = fixtureA->GetBody();
		b2Body* bodyB = fixtureB->GetBody();
		bool nonStatic = bodyA->GetType() != b2_staticBody && bodyB->GetType() != b2_staticBody;
		if (nonStatic)
		{
			++i1;
			b2Swap(m_contacts[i1], m_contacts[i2]);
		}
	}

	// Initialize velocity constraints.
	b2ContactSolver contactSolver(m_contacts, m_contactCount, m_allocator, step.dtRatio);
	contactSolver.WarmStart();
	for (int32 i = 0; i < m_jointCount; ++i)
	{
		m_joints[i]->InitVelocityConstraints(step);
	}

	// Solve velocity constraints.
	for (int32 i = 0; i < step.velocityIterations; ++i)
	{
		for (int32 j = 0; j < m_jointCount; ++j)
		{
			m_joints[j]->SolveVelocityConstraints(step);
		}

		contactSolver.SolveVelocityConstraints();
	}

	// Post-solve (store impulses for warm starting).
	contactSolver.StoreImpulses();

	// Integrate positions.
	for (int32 i = 0; i < m_bodyCount; ++i)
	{
		b2Body* b = m_bodies[i];

		if (b->GetType() == b2_staticBody)
		{
			continue;
		}

		// Check for large velocities.
		b2Vec2 translation = step.dt * b->m_linearVelocity;
		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
		{
			float32 ratio = b2_maxTranslation / translation.Length();
			b->m_linearVelocity *= ratio;
		}

		float32 rotation = step.dt * b->m_angularVelocity;
		if (rotation * rotation > b2_maxRotationSquared)
		{
			float32 ratio = b2_maxRotation / b2Abs(rotation);
			b->m_angularVelocity *= ratio;
		}

		// Store positions for continuous collision.
		b->m_sweep.c0 = b->m_sweep.c;
		b->m_sweep.a0 = b->m_sweep.a;

		// Integrate
		b->m_sweep.c += step.dt * b->m_linearVelocity;
		b->m_sweep.a += step.dt * b->m_angularVelocity;

		// Compute new transform
		b->SynchronizeTransform();

		// Note: shapes are synchronized later.
	}

	// Iterate over constraints.
	for (int32 i = 0; i < step.positionIterations; ++i)
	{
		bool contactsOkay = contactSolver.SolvePositionConstraints(b2_contactBaumgarte);

		bool jointsOkay = true;
		for (int32 i = 0; i < m_jointCount; ++i)
		{
			bool jointOkay = m_joints[i]->SolvePositionConstraints(b2_contactBaumgarte);
			jointsOkay = jointsOkay && jointOkay;
		}

		if (contactsOkay && jointsOkay)
		{
			// Exit early if the position errors are small.
			break;
		}
	}

	Report(contactSolver.m_constraints);

	if (allowSleep)
	{
		float32 minSleepTime = b2_maxFloat;

		const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
		const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;

		for (int32 i = 0; i < m_bodyCount; ++i)
		{
			b2Body* b = m_bodies[i];
			if (b->GetType() == b2_staticBody)
			{
				continue;
			}

			if ((b->m_flags & b2Body::e_autoSleepFlag) == 0)
			{
				b->m_sleepTime = 0.0f;
				minSleepTime = 0.0f;
			}

			if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
				b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
				b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
			{
				b->m_sleepTime = 0.0f;
				minSleepTime = 0.0f;
			}
			else
			{
				b->m_sleepTime += step.dt;
				minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
			}
		}

		if (minSleepTime >= b2_timeToSleep)
		{
			for (int32 i = 0; i < m_bodyCount; ++i)
			{
				b2Body* b = m_bodies[i];
				b->SetAwake(false);
			}
		}
	}
}
bool b2PrismaticJoint::SolvePositionConstraints(const b2SolverData& data)
{
	b2Vec2 cA = data.positions[m_indexA].c;
	float32 aA = data.positions[m_indexA].a;
	b2Vec2 cB = data.positions[m_indexB].c;
	float32 aB = data.positions[m_indexB].a;

	b2Rot qA(aA), qB(aB);

	float32 mA = m_invMassA, mB = m_invMassB;
	float32 iA = m_invIA, iB = m_invIB;

	// Compute fresh Jacobians
	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
	b2Vec2 d = cB + rB - cA - rA;

	b2Vec2 axis = b2Mul(qA, m_localXAxisA);
	float32 a1 = b2Cross(d + rA, axis);
	float32 a2 = b2Cross(rB, axis);
	b2Vec2 perp = b2Mul(qA, m_localYAxisA);

	float32 s1 = b2Cross(d + rA, perp);
	float32 s2 = b2Cross(rB, perp);

	b2Vec3 impulse;
	b2Vec2 C1;
	C1.x = b2Dot(perp, d);
	C1.y = aB - aA - m_referenceAngle;

	float32 linearError = b2Abs(C1.x);
	float32 angularError = b2Abs(C1.y);

	bool active = false;
	float32 C2 = 0.0f;
	if (m_enableLimit)
	{
		float32 translation = b2Dot(axis, d);
		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
		{
			// Prevent large angular corrections
			C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
			linearError = b2Max(linearError, b2Abs(translation));
			active = true;
		}
		else if (translation <= m_lowerTranslation)
		{
			// Prevent large linear corrections and allow some slop.
			C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
			linearError = b2Max(linearError, m_lowerTranslation - translation);
			active = true;
		}
		else if (translation >= m_upperTranslation)
		{
			// Prevent large linear corrections and allow some slop.
			C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
			linearError = b2Max(linearError, translation - m_upperTranslation);
			active = true;
		}
	}

	if (active)
	{
		float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
		float32 k12 = iA * s1 + iB * s2;
		float32 k13 = iA * s1 * a1 + iB * s2 * a2;
		float32 k22 = iA + iB;
		if (k22 == 0.0f)
		{
			// For fixed rotation
			k22 = 1.0f;
		}
		float32 k23 = iA * a1 + iB * a2;
		float32 k33 = mA + mB + iA * a1 * a1 + iB * a2 * a2;

		b2Mat33 K;
		K.ex.Set(k11, k12, k13);
		K.ey.Set(k12, k22, k23);
		K.ez.Set(k13, k23, k33);

		b2Vec3 C;
		C.x = C1.x;
		C.y = C1.y;
		C.z = C2;

		impulse = K.Solve33(-C);
	}
	else
	{
		float32 k11 = mA + mB + iA * s1 * s1 + iB * s2 * s2;
		float32 k12 = iA * s1 + iB * s2;
		float32 k22 = iA + iB;
		if (k22 == 0.0f)
		{
			k22 = 1.0f;
		}

		b2Mat22 K;
		K.ex.Set(k11, k12);
		K.ey.Set(k12, k22);

		b2Vec2 impulse1 = K.Solve(-C1);
		impulse.x = impulse1.x;
		impulse.y = impulse1.y;
		impulse.z = 0.0f;
	}

	b2Vec2 P = impulse.x * perp + impulse.z * axis;
	float32 LA = impulse.x * s1 + impulse.y + impulse.z * a1;
	float32 LB = impulse.x * s2 + impulse.y + impulse.z * a2;

	cA -= mA * P;
	aA -= iA * LA;
	cB += mB * P;
	aB += iB * LB;

	data.positions[m_indexA].c = cA;
	data.positions[m_indexA].a = aA;
	data.positions[m_indexB].c = cB;
	data.positions[m_indexB].a = aB;

	return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
}
void b2PrismaticJoint::SolveVelocityConstraints(const b2SolverData& data)
{
	b2Vec2 vA = data.velocities[m_indexA].v;
	float32 wA = data.velocities[m_indexA].w;
	b2Vec2 vB = data.velocities[m_indexB].v;
	float32 wB = data.velocities[m_indexB].w;

	float32 mA = m_invMassA, mB = m_invMassB;
	float32 iA = m_invIA, iB = m_invIB;

	// Solve linear motor constraint.
	if (m_enableMotor && m_limitState != e_equalLimits)
	{
		float32 Cdot = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
		float32 impulse = m_motorMass * (m_motorSpeed - Cdot);
		float32 oldImpulse = m_motorImpulse;
		float32 maxImpulse = data.step.dt * m_maxMotorForce;
		m_motorImpulse = b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
		impulse = m_motorImpulse - oldImpulse;

		b2Vec2 P = impulse * m_axis;
		float32 LA = impulse * m_a1;
		float32 LB = impulse * m_a2;

		vA -= mA * P;
		wA -= iA * LA;

		vB += mB * P;
		wB += iB * LB;
	}

	b2Vec2 Cdot1;
	Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
	Cdot1.y = wB - wA;

	if (m_enableLimit && m_limitState != e_inactiveLimit)
	{
		// Solve prismatic and limit constraint in block form.
		float32 Cdot2;
		Cdot2 = b2Dot(m_axis, vB - vA) + m_a2 * wB - m_a1 * wA;
		b2Vec3 Cdot(Cdot1.x, Cdot1.y, Cdot2);

		b2Vec3 f1 = m_impulse;
		b2Vec3 df =  m_K.Solve33(-Cdot);
		m_impulse += df;

		if (m_limitState == e_atLowerLimit)
		{
			m_impulse.z = b2Max(m_impulse.z, 0.0f);
		}
		else if (m_limitState == e_atUpperLimit)
		{
			m_impulse.z = b2Min(m_impulse.z, 0.0f);
		}

		// f2(1:2) = invK(1:2,1:2) * (-Cdot(1:2) - K(1:2,3) * (f2(3) - f1(3))) + f1(1:2)
		b2Vec2 b = -Cdot1 - (m_impulse.z - f1.z) * b2Vec2(m_K.ez.x, m_K.ez.y);
		b2Vec2 f2r = m_K.Solve22(b) + b2Vec2(f1.x, f1.y);
		m_impulse.x = f2r.x;
		m_impulse.y = f2r.y;

		df = m_impulse - f1;

		b2Vec2 P = df.x * m_perp + df.z * m_axis;
		float32 LA = df.x * m_s1 + df.y + df.z * m_a1;
		float32 LB = df.x * m_s2 + df.y + df.z * m_a2;

		vA -= mA * P;
		wA -= iA * LA;

		vB += mB * P;
		wB += iB * LB;
	}
	else
	{
		// Limit is inactive, just solve the prismatic constraint in block form.
		b2Vec2 df = m_K.Solve22(-Cdot1);
		m_impulse.x += df.x;
		m_impulse.y += df.y;

		b2Vec2 P = df.x * m_perp;
		float32 LA = df.x * m_s1 + df.y;
		float32 LB = df.x * m_s2 + df.y;

		vA -= mA * P;
		wA -= iA * LA;

		vB += mB * P;
		wB += iB * LB;

		b2Vec2 Cdot10 = Cdot1;

		Cdot1.x = b2Dot(m_perp, vB - vA) + m_s2 * wB - m_s1 * wA;
		Cdot1.y = wB - wA;

		if (b2Abs(Cdot1.x) > 0.01f || b2Abs(Cdot1.y) > 0.01f)
		{
			b2Vec2 test = b2Mul22(m_K, df);
			Cdot1.x += 0.0f;
		}
	}

	data.velocities[m_indexA].v = vA;
	data.velocities[m_indexA].w = wA;
	data.velocities[m_indexB].v = vB;
	data.velocities[m_indexB].w = wB;
}
Exemple #25
0
void b2Island::SolveTOI(const b2TimeStep& subStep, int32 toiIndexA, int32 toiIndexB)
{
	b2Assert(toiIndexA < m_bodyCount);
	b2Assert(toiIndexB < m_bodyCount);

	// Initialize the body state.
	for (int32 i = 0; i < m_bodyCount; ++i)
	{
		b2Body* b = m_bodies[i];
		m_positions[i].c = b->m_sweep.c;
		m_positions[i].a = b->m_sweep.a;
		m_velocities[i].v = b->m_linearVelocity;
		m_velocities[i].w = b->m_angularVelocity;
	}

	b2ContactSolverDef contactSolverDef;
	contactSolverDef.contacts = m_contacts;
	contactSolverDef.count = m_contactCount;
	contactSolverDef.allocator = m_allocator;
	contactSolverDef.step = subStep;
	contactSolverDef.positions = m_positions;
	contactSolverDef.velocities = m_velocities;
	b2ContactSolver contactSolver(&contactSolverDef);

	// Solve position constraints.
	for (int32 i = 0; i < subStep.positionIterations; ++i)
	{
		bool contactsOkay = contactSolver.SolveTOIPositionConstraints(toiIndexA, toiIndexB);
		if (contactsOkay)
		{
			break;
		}
	}

#if 0
	// Is the new position really safe?
	for (int32 i = 0; i < m_contactCount; ++i)
	{
		b2Contact* c = m_contacts[i];
		b2Fixture* fA = c->GetFixtureA();
		b2Fixture* fB = c->GetFixtureB();

		b2Body* bA = fA->GetBody();
		b2Body* bB = fB->GetBody();

		int32 indexA = c->GetChildIndexA();
		int32 indexB = c->GetChildIndexB();

		b2DistanceInput input;
		input.proxyA.Set(fA->GetShape(), indexA);
		input.proxyB.Set(fB->GetShape(), indexB);
		input.transformA = bA->GetTransform();
		input.transformB = bB->GetTransform();
		input.useRadii = false;

		b2DistanceOutput output;
		b2SimplexCache cache;
		cache.count = 0;
		b2Distance(&output, &cache, &input);

		if (output.distance == 0 || cache.count == 3)
		{
			cache.count += 0;
		}
	}
#endif

	// Leap of faith to new safe state.
	m_bodies[toiIndexA]->m_sweep.c0 = m_positions[toiIndexA].c;
	m_bodies[toiIndexA]->m_sweep.a0 = m_positions[toiIndexA].a;
	m_bodies[toiIndexB]->m_sweep.c0 = m_positions[toiIndexB].c;
	m_bodies[toiIndexB]->m_sweep.a0 = m_positions[toiIndexB].a;

	// No warm starting is needed for TOI events because warm
	// starting impulses were applied in the discrete solver.
	contactSolver.InitializeVelocityConstraints();

	// Solve velocity constraints.
	for (int32 i = 0; i < subStep.velocityIterations; ++i)
	{
		contactSolver.SolveVelocityConstraints();
	}

	// Don't store the TOI contact forces for warm starting
	// because they can be quite large.

	float32 h = subStep.dt;

	// Integrate positions
	for (int32 i = 0; i < m_bodyCount; ++i)
	{
		b2Vec2 c = m_positions[i].c;
		float32 a = m_positions[i].a;
		b2Vec2 v = m_velocities[i].v;
		float32 w = m_velocities[i].w;

		// Check for large velocities
		b2Vec2 translation = h * v;
		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
		{
			float32 ratio = b2_maxTranslation / translation.Length();
			v *= ratio;
		}

		float32 rotation = h * w;
		if (rotation * rotation > b2_maxRotationSquared)
		{
			float32 ratio = b2_maxRotation / b2Abs(rotation);
			w *= ratio;
		}

		// Integrate
		c += h * v;
		a += h * w;

		m_positions[i].c = c;
		m_positions[i].a = a;
		m_velocities[i].v = v;
		m_velocities[i].w = w;

		// Sync bodies
		b2Body* body = m_bodies[i];
		body->m_sweep.c = c;
		body->m_sweep.a = a;
		body->m_linearVelocity = v;
		body->m_angularVelocity = w;
		body->SynchronizeTransform();
	}

	Report(contactSolver.m_velocityConstraints);
}
Exemple #26
0
bool b2LineJoint::SolvePositionConstraints(float32 baumgarte)
{
	B2_NOT_USED(baumgarte);

	b2Body* b1 = m_bodyA;
	b2Body* b2 = m_bodyB;

	b2Vec2 c1 = b1->m_sweep.c;
	float32 a1 = b1->m_sweep.a;

	b2Vec2 c2 = b2->m_sweep.c;
	float32 a2 = b2->m_sweep.a;

	// Solve linear limit constraint.
	float32 linearError = 0.0f, angularError = 0.0f;
	bool active = false;
	float32 C2 = 0.0f;

	b2Mat22 R1(a1), R2(a2);

	b2Vec2 r1 = b2Mul(R1, m_localAnchor1 - m_localCenterA);
	b2Vec2 r2 = b2Mul(R2, m_localAnchor2 - m_localCenterB);
	b2Vec2 d = c2 + r2 - c1 - r1;

	if (m_enableLimit)
	{
		m_axis = b2Mul(R1, m_localXAxis1);

		m_a1 = b2Cross(d + r1, m_axis);
		m_a2 = b2Cross(r2, m_axis);

		float32 translation = b2Dot(m_axis, d);
		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
		{
			// Prevent large angular corrections
			C2 = b2Clamp(translation, -b2_maxLinearCorrection, b2_maxLinearCorrection);
			linearError = b2Abs(translation);
			active = true;
		}
		else if (translation <= m_lowerTranslation)
		{
			// Prevent large linear corrections and allow some slop.
			C2 = b2Clamp(translation - m_lowerTranslation + b2_linearSlop, -b2_maxLinearCorrection, 0.0f);
			linearError = m_lowerTranslation - translation;
			active = true;
		}
		else if (translation >= m_upperTranslation)
		{
			// Prevent large linear corrections and allow some slop.
			C2 = b2Clamp(translation - m_upperTranslation - b2_linearSlop, 0.0f, b2_maxLinearCorrection);
			linearError = translation - m_upperTranslation;
			active = true;
		}
	}

	m_perp = b2Mul(R1, m_localYAxis1);

	m_s1 = b2Cross(d + r1, m_perp);
	m_s2 = b2Cross(r2, m_perp);

	b2Vec2 impulse;
	float32 C1;
	C1 = b2Dot(m_perp, d);

	linearError = b2Max(linearError, b2Abs(C1));
	angularError = 0.0f;

	if (active)
	{
		float32 m1 = m_invMassA, m2 = m_invMassB;
		float32 i1 = m_invIA, i2 = m_invIB;

		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;
		float32 k12 = i1 * m_s1 * m_a1 + i2 * m_s2 * m_a2;
		float32 k22 = m1 + m2 + i1 * m_a1 * m_a1 + i2 * m_a2 * m_a2;

		m_K.col1.Set(k11, k12);
		m_K.col2.Set(k12, k22);

		b2Vec2 C;
		C.x = C1;
		C.y = C2;

		impulse = m_K.Solve(-C);
	}
	else
	{
		float32 m1 = m_invMassA, m2 = m_invMassB;
		float32 i1 = m_invIA, i2 = m_invIB;

		float32 k11 = m1 + m2 + i1 * m_s1 * m_s1 + i2 * m_s2 * m_s2;

		float32 impulse1;
		if (k11 != 0.0f)
		{
			impulse1 = - C1 / k11;
		}
		else
		{
			impulse1 = 0.0f;
		}

		impulse.x = impulse1;
		impulse.y = 0.0f;
	}

	b2Vec2 P = impulse.x * m_perp + impulse.y * m_axis;
	float32 L1 = impulse.x * m_s1 + impulse.y * m_a1;
	float32 L2 = impulse.x * m_s2 + impulse.y * m_a2;

	c1 -= m_invMassA * P;
	a1 -= m_invIA * L1;
	c2 += m_invMassB * P;
	a2 += m_invIB * L2;

	// TODO_ERIN remove need for this.
	b1->m_sweep.c = c1;
	b1->m_sweep.a = a1;
	b2->m_sweep.c = c2;
	b2->m_sweep.a = a2;
	b1->SynchronizeTransform();
	b2->SynchronizeTransform();

	return linearError <= b2_linearSlop && angularError <= b2_angularSlop;
}
Exemple #27
0
void b2Island::Solve(b2Profile* profile, const b2TimeStep& step, const b2Vec2& gravity, bool allowSleep)
{
	b2Timer timer;

	float32 h = step.dt;

	// Integrate velocities and apply damping. Initialize the body state.
	for (int32 i = 0; i < m_bodyCount; ++i)
	{
		b2Body* b = m_bodies[i];

		b2Vec2 c = b->m_sweep.c;
		float32 a = b->m_sweep.a;
		b2Vec2 v = b->m_linearVelocity;
		float32 w = b->m_angularVelocity;

		// Store positions for continuous collision.
		b->m_sweep.c0 = b->m_sweep.c;
		b->m_sweep.a0 = b->m_sweep.a;

		if (b->m_type == b2_dynamicBody)
		{
			// Integrate velocities.
			v += h * (b->m_gravityScale * gravity + b->m_invMass * b->m_force);
			w += h * b->m_invI * b->m_torque;

			// Apply damping.
			// ODE: dv/dt + c * v = 0
			// Solution: v(t) = v0 * exp(-c * t)
			// Time step: v(t + dt) = v0 * exp(-c * (t + dt)) = v0 * exp(-c * t) * exp(-c * dt) = v * exp(-c * dt)
			// v2 = exp(-c * dt) * v1
			// Taylor expansion:
			// v2 = (1.0f - c * dt) * v1
			v *= b2Clamp(1.0f - h * b->m_linearDamping, 0.0f, 1.0f);
			w *= b2Clamp(1.0f - h * b->m_angularDamping, 0.0f, 1.0f);
		}

		m_positions[i].c = c;
		m_positions[i].a = a;
		m_velocities[i].v = v;
		m_velocities[i].w = w;
	}

	timer.Reset();

	// Solver data
	b2SolverData solverData;
	solverData.step = step;
	solverData.positions = m_positions;
	solverData.velocities = m_velocities;

	// Initialize velocity constraints.
	b2ContactSolverDef contactSolverDef;
	contactSolverDef.step = step;
	contactSolverDef.contacts = m_contacts;
	contactSolverDef.count = m_contactCount;
	contactSolverDef.positions = m_positions;
	contactSolverDef.velocities = m_velocities;
	contactSolverDef.allocator = m_allocator;

	b2ContactSolver contactSolver(&contactSolverDef);
	contactSolver.InitializeVelocityConstraints();

	if (step.warmStarting)
	{
		contactSolver.WarmStart();
	}
	
	for (int32 i = 0; i < m_jointCount; ++i)
	{
		m_joints[i]->InitVelocityConstraints(solverData);
	}

	profile->solveInit = timer.GetMilliseconds();

	// Solve velocity constraints
	timer.Reset();
	for (int32 i = 0; i < step.velocityIterations; ++i)
	{
		for (int32 j = 0; j < m_jointCount; ++j)
		{
			m_joints[j]->SolveVelocityConstraints(solverData);
		}

		contactSolver.SolveVelocityConstraints();
	}

	// Store impulses for warm starting
	contactSolver.StoreImpulses();
	profile->solveVelocity = timer.GetMilliseconds();

	// Integrate positions
	for (int32 i = 0; i < m_bodyCount; ++i)
	{
		b2Vec2 c = m_positions[i].c;
		float32 a = m_positions[i].a;
		b2Vec2 v = m_velocities[i].v;
		float32 w = m_velocities[i].w;

		// Check for large velocities
		b2Vec2 translation = h * v;
		if (b2Dot(translation, translation) > b2_maxTranslationSquared)
		{
			float32 ratio = b2_maxTranslation / translation.Length();
			v *= ratio;
		}

		float32 rotation = h * w;
		if (rotation * rotation > b2_maxRotationSquared)
		{
			float32 ratio = b2_maxRotation / b2Abs(rotation);
			w *= ratio;
		}

		// Integrate
		c += h * v;
		a += h * w;

		m_positions[i].c = c;
		m_positions[i].a = a;
		m_velocities[i].v = v;
		m_velocities[i].w = w;
	}

	// Solve position constraints
	timer.Reset();
	bool positionSolved = false;
	for (int32 i = 0; i < step.positionIterations; ++i)
	{
		bool contactsOkay = contactSolver.SolvePositionConstraints();

		bool jointsOkay = true;
		for (int32 i = 0; i < m_jointCount; ++i)
		{
			bool jointOkay = m_joints[i]->SolvePositionConstraints(solverData);
			jointsOkay = jointsOkay && jointOkay;
		}

		if (contactsOkay && jointsOkay)
		{
			// Exit early if the position errors are small.
			positionSolved = true;
			break;
		}
	}

	// Copy state buffers back to the bodies
	for (int32 i = 0; i < m_bodyCount; ++i)
	{
		b2Body* body = m_bodies[i];
		body->m_sweep.c = m_positions[i].c;
		body->m_sweep.a = m_positions[i].a;
		body->m_linearVelocity = m_velocities[i].v;
		body->m_angularVelocity = m_velocities[i].w;
		body->SynchronizeTransform();
	}

	profile->solvePosition = timer.GetMilliseconds();

	Report(contactSolver.m_velocityConstraints);

	if (allowSleep)
	{
		float32 minSleepTime = b2_maxFloat;

		const float32 linTolSqr = b2_linearSleepTolerance * b2_linearSleepTolerance;
		const float32 angTolSqr = b2_angularSleepTolerance * b2_angularSleepTolerance;

		for (int32 i = 0; i < m_bodyCount; ++i)
		{
			b2Body* b = m_bodies[i];
			if (b->GetType() == b2_staticBody)
			{
				continue;
			}

			if ((b->m_flags & b2Body::e_autoSleepFlag) == 0 ||
				b->m_angularVelocity * b->m_angularVelocity > angTolSqr ||
				b2Dot(b->m_linearVelocity, b->m_linearVelocity) > linTolSqr)
			{
				b->m_sleepTime = 0.0f;
				minSleepTime = 0.0f;
			}
			else
			{
				b->m_sleepTime += h;
				minSleepTime = b2Min(minSleepTime, b->m_sleepTime);
			}
		}

		if (minSleepTime >= b2_timeToSleep && positionSolved)
		{
			for (int32 i = 0; i < m_bodyCount; ++i)
			{
				b2Body* b = m_bodies[i];
				b->SetAwake(false);
			}
		}
	}
}
Exemple #28
0
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
{
	++b2_toiCalls;

	output->state = b2TOIOutput::e_unknown;
	output->t = input->tMax;

	const b2DistanceProxy* proxyA = &input->proxyA;
	const b2DistanceProxy* proxyB = &input->proxyB;

	b2Sweep sweepA = input->sweepA;
	b2Sweep sweepB = input->sweepB;

	// Large rotations can make the root finder fail, so we normalize the
	// sweep angles.
	sweepA.Normalize();
	sweepB.Normalize();

	float32 tMax = input->tMax;

	float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
	float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
	float32 tolerance = 0.25f * b2_linearSlop;
	b2Assert(target > tolerance);

	float32 t1 = 0.0f;
	const int32 k_maxIterations = 20;	// TODO_ERIN b2Settings
	int32 iter = 0;

	// Prepare input for distance query.
	b2SimplexCache cache;
	cache.count = 0;
	b2DistanceInput distanceInput;
	distanceInput.proxyA = input->proxyA;
	distanceInput.proxyB = input->proxyB;
	distanceInput.useRadii = false;

	// The outer loop progressively attempts to compute new separating axes.
	// This loop terminates when an axis is repeated (no progress is made).
	for(;;)
	{
		b2Transform xfA, xfB;
		sweepA.GetTransform(&xfA, t1);
		sweepB.GetTransform(&xfB, t1);

		// Get the distance between shapes. We can also use the results
		// to get a separating axis.
		distanceInput.transformA = xfA;
		distanceInput.transformB = xfB;
		b2DistanceOutput distanceOutput;
		b2Distance(&distanceOutput, &cache, &distanceInput);

		// If the shapes are overlapped, we give up on continuous collision.
		if (distanceOutput.distance <= 0.0f)
		{
			// Failure!
			output->state = b2TOIOutput::e_overlapped;
			output->t = 0.0f;
			break;
		}

		if (distanceOutput.distance < target + tolerance)
		{
			// Victory!
			output->state = b2TOIOutput::e_touching;
			output->t = t1;
			break;
		}

		// Initialize the separating axis.
		b2SeparationFunction fcn;
		fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB);
#if 0
		// Dump the curve seen by the root finder
		{
			const int32 N = 100;
			float32 dx = 1.0f / N;
			float32 xs[N+1];
			float32 fs[N+1];

			float32 x = 0.0f;

			for (int32 i = 0; i <= N; ++i)
			{
				sweepA.GetTransform(&xfA, x);
				sweepB.GetTransform(&xfB, x);
				float32 f = fcn.Evaluate(xfA, xfB) - target;

				printf("%g %g\n", x, f);

				xs[i] = x;
				fs[i] = f;

				x += dx;
			}
		}
#endif

		// Compute the TOI on the separating axis. We do this by successively
		// resolving the deepest point. This loop is bounded by the number of vertices.
		bool done = false;
		float32 t2 = tMax;
		int32 pushBackIter = 0;
		for (;;)
		{
			// Find the deepest point at t2. Store the witness point indices.
			int32 indexA, indexB;
			float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);

			// Is the final configuration separated?
			if (s2 > target + tolerance)
			{
				// Victory!
				output->state = b2TOIOutput::e_separated;
				output->t = tMax;
				done = true;
				break;
			}

			// Has the separation reached tolerance?
			if (s2 > target - tolerance)
			{
				// Advance the sweeps
				t1 = t2;
				break;
			}

			// Compute the initial separation of the witness points.
			float32 s1 = fcn.Evaluate(indexA, indexB, t1);

			// Check for initial overlap. This might happen if the root finder
			// runs out of iterations.
			if (s1 < target - tolerance)
			{
				output->state = b2TOIOutput::e_failed;
				output->t = t1;
				done = true;
				break;
			}

			// Check for touching
			if (s1 <= target + tolerance)
			{
				// Victory! t1 should hold the TOI (could be 0.0).
				output->state = b2TOIOutput::e_touching;
				output->t = t1;
				done = true;
				break;
			}

			// Compute 1D root of: f(x) - target = 0
			int32 rootIterCount = 0;
			float32 a1 = t1, a2 = t2;
			for (;;)
			{
				// Use a mix of the secant rule and bisection.
				float32 t;
				if (rootIterCount & 1)
				{
					// Secant rule to improve convergence.
					t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
				}
				else
				{
					// Bisection to guarantee progress.
					t = 0.5f * (a1 + a2);
				}

				float32 s = fcn.Evaluate(indexA, indexB, t);

				if (b2Abs(s - target) < tolerance)
				{
					// t2 holds a tentative value for t1
					t2 = t;
					break;
				}

				// Ensure we continue to bracket the root.
				if (s > target)
				{
					a1 = t;
					s1 = s;
				}
				else
				{
					a2 = t;
					s2 = s;
				}

				++rootIterCount;
				++b2_toiRootIters;

				if (rootIterCount == 50)
				{
					break;
				}
			}

			b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);

			++pushBackIter;

			if (pushBackIter == b2_maxPolygonVertices)
			{
				break;
			}
		}

		++iter;
		++b2_toiIters;

		if (done)
		{
			break;
		}

		if (iter == k_maxIterations)
		{
			// Root finder got stuck. Semi-victory.
			output->state = b2TOIOutput::e_failed;
			output->t = t1;
			break;
		}
	}

	b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
}
Exemple #29
0
bool b2WeldJoint::SolvePositionConstraints(const b2SolverData& data)
{
    b2Vec2 cA = data.positions[m_indexA].c;
    float32 aA = data.positions[m_indexA].a;
    b2Vec2 cB = data.positions[m_indexB].c;
    float32 aB = data.positions[m_indexB].a;

    b2Rot qA(aA), qB(aB);

    float32 mA = m_invMassA, mB = m_invMassB;
    float32 iA = m_invIA, iB = m_invIB;

    b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
    b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);

    float32 positionError, angularError;

    b2Mat33 K;
    K.ex.x = mA + mB + rA.y * rA.y * iA + rB.y * rB.y * iB;
    K.ey.x = -rA.y * rA.x * iA - rB.y * rB.x * iB;
    K.ez.x = -rA.y * iA - rB.y * iB;
    K.ex.y = K.ey.x;
    K.ey.y = mA + mB + rA.x * rA.x * iA + rB.x * rB.x * iB;
    K.ez.y = rA.x * iA + rB.x * iB;
    K.ex.z = K.ez.x;
    K.ey.z = K.ez.y;
    K.ez.z = iA + iB;

    if (m_frequencyHz > 0.0f)
    {
        b2Vec2 C1 =  cB + rB - cA - rA;

        positionError = C1.Length();
        angularError = 0.0f;

        b2Vec2 P = -K.Solve22(C1);

        cA -= mA * P;
        aA -= iA * b2Cross(rA, P);

        cB += mB * P;
        aB += iB * b2Cross(rB, P);
    }
    else
    {
        b2Vec2 C1 =  cB + rB - cA - rA;
        float32 C2 = aB - aA - m_referenceAngle;

        positionError = C1.Length();
        angularError = b2Abs(C2);

        b2Vec3 C(C1.x, C1.y, C2);
    
        b2Vec3 impulse = -K.Solve33(C);
        b2Vec2 P(impulse.x, impulse.y);

        cA -= mA * P;
        aA -= iA * (b2Cross(rA, P) + impulse.z);

        cB += mB * P;
        aB += iB * (b2Cross(rB, P) + impulse.z);
    }

    data.positions[m_indexA].c = cA;
    data.positions[m_indexA].a = aA;
    data.positions[m_indexB].c = cB;
    data.positions[m_indexB].a = aB;

    return positionError <= b2_linearSlop && angularError <= b2_angularSlop;
}
void b2PrismaticJoint::InitVelocityConstraints(const b2SolverData& data)
{
	m_indexA = m_bodyA->m_islandIndex;
	m_indexB = m_bodyB->m_islandIndex;
	m_localCenterA = m_bodyA->m_sweep.localCenter;
	m_localCenterB = m_bodyB->m_sweep.localCenter;
	m_invMassA = m_bodyA->m_invMass;
	m_invMassB = m_bodyB->m_invMass;
	m_invIA = m_bodyA->m_invI;
	m_invIB = m_bodyB->m_invI;

	b2Vec2 cA = data.positions[m_indexA].c;
	float32 aA = data.positions[m_indexA].a;
	b2Vec2 vA = data.velocities[m_indexA].v;
	float32 wA = data.velocities[m_indexA].w;

	b2Vec2 cB = data.positions[m_indexB].c;
	float32 aB = data.positions[m_indexB].a;
	b2Vec2 vB = data.velocities[m_indexB].v;
	float32 wB = data.velocities[m_indexB].w;

	b2Rot qA(aA), qB(aB);

	// Compute the effective masses.
	b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
	b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
	b2Vec2 d = (cB - cA) + rB - rA;

	float32 mA = m_invMassA, mB = m_invMassB;
	float32 iA = m_invIA, iB = m_invIB;

	// Compute motor Jacobian and effective mass.
	{
		m_axis = b2Mul(qA, m_localXAxisA);
		m_a1 = b2Cross(d + rA, m_axis);
		m_a2 = b2Cross(rB, m_axis);

		m_motorMass = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;
		if (m_motorMass > 0.0f)
		{
			m_motorMass = 1.0f / m_motorMass;
		}
	}

	// Prismatic constraint.
	{
		m_perp = b2Mul(qA, m_localYAxisA);

		m_s1 = b2Cross(d + rA, m_perp);
		m_s2 = b2Cross(rB, m_perp);

		float32 k11 = mA + mB + iA * m_s1 * m_s1 + iB * m_s2 * m_s2;
		float32 k12 = iA * m_s1 + iB * m_s2;
		float32 k13 = iA * m_s1 * m_a1 + iB * m_s2 * m_a2;
		float32 k22 = iA + iB;
		if (k22 == 0.0f)
		{
			// For bodies with fixed rotation.
			k22 = 1.0f;
		}
		float32 k23 = iA * m_a1 + iB * m_a2;
		float32 k33 = mA + mB + iA * m_a1 * m_a1 + iB * m_a2 * m_a2;

		m_K.ex.Set(k11, k12, k13);
		m_K.ey.Set(k12, k22, k23);
		m_K.ez.Set(k13, k23, k33);
	}

	// Compute motor and limit terms.
	if (m_enableLimit)
	{
		float32 jointTranslation = b2Dot(m_axis, d);
		if (b2Abs(m_upperTranslation - m_lowerTranslation) < 2.0f * b2_linearSlop)
		{
			m_limitState = e_equalLimits;
		}
		else if (jointTranslation <= m_lowerTranslation)
		{
			if (m_limitState != e_atLowerLimit)
			{
				m_limitState = e_atLowerLimit;
				m_impulse.z = 0.0f;
			}
		}
		else if (jointTranslation >= m_upperTranslation)
		{
			if (m_limitState != e_atUpperLimit)
			{
				m_limitState = e_atUpperLimit;
				m_impulse.z = 0.0f;
			}
		}
		else
		{
			m_limitState = e_inactiveLimit;
			m_impulse.z = 0.0f;
		}
	}
	else
	{
		m_limitState = e_inactiveLimit;
		m_impulse.z = 0.0f;
	}

	if (m_enableMotor == false)
	{
		m_motorImpulse = 0.0f;
	}

	if (data.step.warmStarting)
	{
		// Account for variable time step.
		m_impulse *= data.step.dtRatio;
		m_motorImpulse *= data.step.dtRatio;

		b2Vec2 P = m_impulse.x * m_perp + (m_motorImpulse + m_impulse.z) * m_axis;
		float32 LA = m_impulse.x * m_s1 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a1;
		float32 LB = m_impulse.x * m_s2 + m_impulse.y + (m_motorImpulse + m_impulse.z) * m_a2;

		vA -= mA * P;
		wA -= iA * LA;

		vB += mB * P;
		wB += iB * LB;
	}
	else
	{
		m_impulse.SetZero();
		m_motorImpulse = 0.0f;
	}

	data.velocities[m_indexA].v = vA;
	data.velocities[m_indexA].w = wA;
	data.velocities[m_indexB].v = vB;
	data.velocities[m_indexB].w = wB;
}