예제 #1
0
void btHingeConstraint::getInfo1(btConstraintInfo1* info)
{
    if (m_useSolveConstraintObsolete)
    {
        info->m_numConstraintRows = 0;
        info->nub = 0;
    }
    else
    {
        info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
        info->nub = 1;
        //prepare constraint
        testLimit();
        if(getSolveLimit() || getEnableAngularMotor())
        {
            info->m_numConstraintRows++; // limit 3rd anguar as well
            info->nub--;
        }
    }
}
void btHingeConstraint::getInfo1(btConstraintInfo1* info)
{
	if (m_useSolveConstraintObsolete)
	{
		info->m_numConstraintRows = 0;
		info->nub = 0;
	}
	else
	{
		info->m_numConstraintRows = 5; // Fixed 3 linear + 2 angular
		info->nub = 1; 
		//always add the row, to avoid computation (data is not available yet)
		//prepare constraint
		testLimit(m_rbA.getCenterOfMassTransform(),m_rbB.getCenterOfMassTransform());
		if(getSolveLimit() || getEnableAngularMotor())
		{
			info->m_numConstraintRows++; // limit 3rd anguar as well
			info->nub--; 
		}

	}
}
void btHingeConstraint::getInfo2InternalUsingFrameOffset(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
{
	btAssert(!m_useSolveConstraintObsolete);
	int i, s = info->rowskip;
	// transforms in world space
	btTransform trA = transA*m_rbAFrame;
	btTransform trB = transB*m_rbBFrame;
	// pivot point
	btVector3 pivotAInW = trA.getOrigin();
	btVector3 pivotBInW = trB.getOrigin();
#if 1
	// difference between frames in WCS
	btVector3 ofs = trB.getOrigin() - trA.getOrigin();
	// now get weight factors depending on masses
	btScalar miA = getRigidBodyA().getInvMass();
	btScalar miB = getRigidBodyB().getInvMass();
	bool hasStaticBody = (miA < SIMD_EPSILON) || (miB < SIMD_EPSILON);
	btScalar miS = miA + miB;
	btScalar factA, factB;
	if(miS > btScalar(0.f))
	{
		factA = miB / miS;
	}
	else 
	{
		factA = btScalar(0.5f);
	}
	factB = btScalar(1.0f) - factA;
	// get the desired direction of hinge axis
	// as weighted sum of Z-orthos of frameA and frameB in WCS
	btVector3 ax1A = trA.getBasis().getColumn(2);
	btVector3 ax1B = trB.getBasis().getColumn(2);
	btVector3 ax1 = ax1A * factA + ax1B * factB;
	ax1.normalize();
	// fill first 3 rows 
	// we want: velA + wA x relA == velB + wB x relB
	btTransform bodyA_trans = transA;
	btTransform bodyB_trans = transB;
	int s0 = 0;
	int s1 = s;
	int s2 = s * 2;
	int nrow = 2; // last filled row
	btVector3 tmpA, tmpB, relA, relB, p, q;
	// get vector from bodyB to frameB in WCS
	relB = trB.getOrigin() - bodyB_trans.getOrigin();
	// get its projection to hinge axis
	btVector3 projB = ax1 * relB.dot(ax1);
	// get vector directed from bodyB to hinge axis (and orthogonal to it)
	btVector3 orthoB = relB - projB;
	// same for bodyA
	relA = trA.getOrigin() - bodyA_trans.getOrigin();
	btVector3 projA = ax1 * relA.dot(ax1);
	btVector3 orthoA = relA - projA;
	btVector3 totalDist = projA - projB;
	// get offset vectors relA and relB
	relA = orthoA + totalDist * factA;
	relB = orthoB - totalDist * factB;
	// now choose average ortho to hinge axis
	p = orthoB * factA + orthoA * factB;
	btScalar len2 = p.length2();
	if(len2 > SIMD_EPSILON)
	{
		p /= btSqrt(len2);
	}
	else
	{
		p = trA.getBasis().getColumn(1);
	}
	// make one more ortho
	q = ax1.cross(p);
	// fill three rows
	tmpA = relA.cross(p);
	tmpB = relB.cross(p);
    for (i=0; i<3; i++) info->m_J1angularAxis[s0+i] = tmpA[i];
    for (i=0; i<3; i++) info->m_J2angularAxis[s0+i] = -tmpB[i];
	tmpA = relA.cross(q);
	tmpB = relB.cross(q);
	if(hasStaticBody && getSolveLimit())
	{ // to make constraint between static and dynamic objects more rigid
		// remove wA (or wB) from equation if angular limit is hit
		tmpB *= factB;
		tmpA *= factA;
	}
	for (i=0; i<3; i++) info->m_J1angularAxis[s1+i] = tmpA[i];
    for (i=0; i<3; i++) info->m_J2angularAxis[s1+i] = -tmpB[i];
	tmpA = relA.cross(ax1);
	tmpB = relB.cross(ax1);
	if(hasStaticBody)
	{ // to make constraint between static and dynamic objects more rigid
		// remove wA (or wB) from equation
		tmpB *= factB;
		tmpA *= factA;
	}
	for (i=0; i<3; i++) info->m_J1angularAxis[s2+i] = tmpA[i];
    for (i=0; i<3; i++) info->m_J2angularAxis[s2+i] = -tmpB[i];

	for (i=0; i<3; i++) info->m_J1linearAxis[s0+i] = p[i];
	for (i=0; i<3; i++) info->m_J1linearAxis[s1+i] = q[i];
	for (i=0; i<3; i++) info->m_J1linearAxis[s2+i] = ax1[i];
	// compute three elements of right hand side
	btScalar k = info->fps * info->erp;
	btScalar rhs = k * p.dot(ofs);
	info->m_constraintError[s0] = rhs;
	rhs = k * q.dot(ofs);
	info->m_constraintError[s1] = rhs;
	rhs = k * ax1.dot(ofs);
	info->m_constraintError[s2] = rhs;
	// the hinge axis should be the only unconstrained
	// rotational axis, the angular velocity of the two bodies perpendicular to
	// the hinge axis should be equal. thus the constraint equations are
	//    p*w1 - p*w2 = 0
	//    q*w1 - q*w2 = 0
	// where p and q are unit vectors normal to the hinge axis, and w1 and w2
	// are the angular velocity vectors of the two bodies.
	int s3 = 3 * s;
	int s4 = 4 * s;
	info->m_J1angularAxis[s3 + 0] = p[0];
	info->m_J1angularAxis[s3 + 1] = p[1];
	info->m_J1angularAxis[s3 + 2] = p[2];
	info->m_J1angularAxis[s4 + 0] = q[0];
	info->m_J1angularAxis[s4 + 1] = q[1];
	info->m_J1angularAxis[s4 + 2] = q[2];

	info->m_J2angularAxis[s3 + 0] = -p[0];
	info->m_J2angularAxis[s3 + 1] = -p[1];
	info->m_J2angularAxis[s3 + 2] = -p[2];
	info->m_J2angularAxis[s4 + 0] = -q[0];
	info->m_J2angularAxis[s4 + 1] = -q[1];
	info->m_J2angularAxis[s4 + 2] = -q[2];
	// compute the right hand side of the constraint equation. set relative
	// body velocities along p and q to bring the hinge back into alignment.
	// if ax1A,ax1B are the unit length hinge axes as computed from bodyA and
	// bodyB, we need to rotate both bodies along the axis u = (ax1 x ax2).
	// if "theta" is the angle between ax1 and ax2, we need an angular velocity
	// along u to cover angle erp*theta in one step :
	//   |angular_velocity| = angle/time = erp*theta / stepsize
	//                      = (erp*fps) * theta
	//    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
	//                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
	// ...as ax1 and ax2 are unit length. if theta is smallish,
	// theta ~= sin(theta), so
	//    angular_velocity  = (erp*fps) * (ax1 x ax2)
	// ax1 x ax2 is in the plane space of ax1, so we project the angular
	// velocity to p and q to find the right hand side.
	k = info->fps * info->erp;
	btVector3 u = ax1A.cross(ax1B);
	info->m_constraintError[s3] = k * u.dot(p);
	info->m_constraintError[s4] = k * u.dot(q);
#endif
	// check angular limits
	nrow = 4; // last filled row
	int srow;
	btScalar limit_err = btScalar(0.0);
	int limit = 0;
	if(getSolveLimit())
	{
		limit_err = m_correction * m_referenceSign;
		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
	}
	// if the hinge has joint limits or motor, add in the extra row
	int powered = 0;
	if(getEnableAngularMotor())
	{
		powered = 1;
	}
	if(limit || powered) 
	{
		nrow++;
		srow = nrow * info->rowskip;
		info->m_J1angularAxis[srow+0] = ax1[0];
		info->m_J1angularAxis[srow+1] = ax1[1];
		info->m_J1angularAxis[srow+2] = ax1[2];

		info->m_J2angularAxis[srow+0] = -ax1[0];
		info->m_J2angularAxis[srow+1] = -ax1[1];
		info->m_J2angularAxis[srow+2] = -ax1[2];

		btScalar lostop = getLowerLimit();
		btScalar histop = getUpperLimit();
		if(limit && (lostop == histop))
		{  // the joint motor is ineffective
			powered = 0;
		}
		info->m_constraintError[srow] = btScalar(0.0f);
		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
		if(powered)
		{
			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
			{
				info->cfm[srow] = m_normalCFM;
			}
			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
			info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
			info->m_upperLimit[srow] =   m_maxMotorImpulse;
		}
		if(limit)
		{
			k = info->fps * currERP;
			info->m_constraintError[srow] += k * limit_err;
			if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
			{
				info->cfm[srow] = m_stopCFM;
			}
			if(lostop == histop) 
			{
				// limited low and high simultaneously
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			else if(limit == 1) 
			{ // low limit
				info->m_lowerLimit[srow] = 0;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			else 
			{ // high limit
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = 0;
			}
			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
			btScalar bounce = m_relaxationFactor;
			if(bounce > btScalar(0.0))
			{
				btScalar vel = angVelA.dot(ax1);
				vel -= angVelB.dot(ax1);
				// only apply bounce if the velocity is incoming, and if the
				// resulting c[] exceeds what we already have.
				if(limit == 1)
				{	// low limit
					if(vel < 0)
					{
						btScalar newc = -bounce * vel;
						if(newc > info->m_constraintError[srow])
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
				else
				{	// high limit - all those computations are reversed
					if(vel > 0)
					{
						btScalar newc = -bounce * vel;
						if(newc < info->m_constraintError[srow])
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
			}
			info->m_constraintError[srow] *= m_biasFactor;
		} // if(limit)
	} // if angular limit or powered
}
void btHingeConstraint::getInfo2Internal(btConstraintInfo2* info, const btTransform& transA,const btTransform& transB,const btVector3& angVelA,const btVector3& angVelB)
{

	btAssert(!m_useSolveConstraintObsolete);
	int i, skip = info->rowskip;
	// transforms in world space
	btTransform trA = transA*m_rbAFrame;
	btTransform trB = transB*m_rbBFrame;
	// pivot point
	btVector3 pivotAInW = trA.getOrigin();
	btVector3 pivotBInW = trB.getOrigin();
#if 0
	if (0)
	{
		for (i=0;i<6;i++)
		{
			info->m_J1linearAxis[i*skip]=0;
			info->m_J1linearAxis[i*skip+1]=0;
			info->m_J1linearAxis[i*skip+2]=0;

			info->m_J1angularAxis[i*skip]=0;
			info->m_J1angularAxis[i*skip+1]=0;
			info->m_J1angularAxis[i*skip+2]=0;

			info->m_J2angularAxis[i*skip]=0;
			info->m_J2angularAxis[i*skip+1]=0;
			info->m_J2angularAxis[i*skip+2]=0;

			info->m_constraintError[i*skip]=0.f;
		}
	}
#endif //#if 0
	// linear (all fixed)
    info->m_J1linearAxis[0] = 1;
    info->m_J1linearAxis[skip + 1] = 1;
    info->m_J1linearAxis[2 * skip + 2] = 1;
	




	btVector3 a1 = pivotAInW - transA.getOrigin();
	{
		btVector3* angular0 = (btVector3*)(info->m_J1angularAxis);
		btVector3* angular1 = (btVector3*)(info->m_J1angularAxis + skip);
		btVector3* angular2 = (btVector3*)(info->m_J1angularAxis + 2 * skip);
		btVector3 a1neg = -a1;
		a1neg.getSkewSymmetricMatrix(angular0,angular1,angular2);
	}
	btVector3 a2 = pivotBInW - transB.getOrigin();
	{
		btVector3* angular0 = (btVector3*)(info->m_J2angularAxis);
		btVector3* angular1 = (btVector3*)(info->m_J2angularAxis + skip);
		btVector3* angular2 = (btVector3*)(info->m_J2angularAxis + 2 * skip);
		a2.getSkewSymmetricMatrix(angular0,angular1,angular2);
	}
	// linear RHS
    btScalar k = info->fps * info->erp;
	for(i = 0; i < 3; i++)
    {
        info->m_constraintError[i * skip] = k * (pivotBInW[i] - pivotAInW[i]);
    }
	// make rotations around X and Y equal
	// the hinge axis should be the only unconstrained
	// rotational axis, the angular velocity of the two bodies perpendicular to
	// the hinge axis should be equal. thus the constraint equations are
	//    p*w1 - p*w2 = 0
	//    q*w1 - q*w2 = 0
	// where p and q are unit vectors normal to the hinge axis, and w1 and w2
	// are the angular velocity vectors of the two bodies.
	// get hinge axis (Z)
	btVector3 ax1 = trA.getBasis().getColumn(2);
	// get 2 orthos to hinge axis (X, Y)
	btVector3 p = trA.getBasis().getColumn(0);
	btVector3 q = trA.getBasis().getColumn(1);
	// set the two hinge angular rows 
    int s3 = 3 * info->rowskip;
    int s4 = 4 * info->rowskip;

	info->m_J1angularAxis[s3 + 0] = p[0];
	info->m_J1angularAxis[s3 + 1] = p[1];
	info->m_J1angularAxis[s3 + 2] = p[2];
	info->m_J1angularAxis[s4 + 0] = q[0];
	info->m_J1angularAxis[s4 + 1] = q[1];
	info->m_J1angularAxis[s4 + 2] = q[2];

	info->m_J2angularAxis[s3 + 0] = -p[0];
	info->m_J2angularAxis[s3 + 1] = -p[1];
	info->m_J2angularAxis[s3 + 2] = -p[2];
	info->m_J2angularAxis[s4 + 0] = -q[0];
	info->m_J2angularAxis[s4 + 1] = -q[1];
	info->m_J2angularAxis[s4 + 2] = -q[2];
    // compute the right hand side of the constraint equation. set relative
    // body velocities along p and q to bring the hinge back into alignment.
    // if ax1,ax2 are the unit length hinge axes as computed from body1 and
    // body2, we need to rotate both bodies along the axis u = (ax1 x ax2).
    // if `theta' is the angle between ax1 and ax2, we need an angular velocity
    // along u to cover angle erp*theta in one step :
    //   |angular_velocity| = angle/time = erp*theta / stepsize
    //                      = (erp*fps) * theta
    //    angular_velocity  = |angular_velocity| * (ax1 x ax2) / |ax1 x ax2|
    //                      = (erp*fps) * theta * (ax1 x ax2) / sin(theta)
    // ...as ax1 and ax2 are unit length. if theta is smallish,
    // theta ~= sin(theta), so
    //    angular_velocity  = (erp*fps) * (ax1 x ax2)
    // ax1 x ax2 is in the plane space of ax1, so we project the angular
    // velocity to p and q to find the right hand side.
    btVector3 ax2 = trB.getBasis().getColumn(2);
	btVector3 u = ax1.cross(ax2);
	info->m_constraintError[s3] = k * u.dot(p);
	info->m_constraintError[s4] = k * u.dot(q);
	// check angular limits
	int nrow = 4; // last filled row
	int srow;
	btScalar limit_err = btScalar(0.0);
	int limit = 0;
	if(getSolveLimit())
	{
		limit_err = m_correction * m_referenceSign;
		limit = (limit_err > btScalar(0.0)) ? 1 : 2;
	}
	// if the hinge has joint limits or motor, add in the extra row
	int powered = 0;
	if(getEnableAngularMotor())
	{
		powered = 1;
	}
	if(limit || powered) 
	{
		nrow++;
		srow = nrow * info->rowskip;
		info->m_J1angularAxis[srow+0] = ax1[0];
		info->m_J1angularAxis[srow+1] = ax1[1];
		info->m_J1angularAxis[srow+2] = ax1[2];

		info->m_J2angularAxis[srow+0] = -ax1[0];
		info->m_J2angularAxis[srow+1] = -ax1[1];
		info->m_J2angularAxis[srow+2] = -ax1[2];

		btScalar lostop = getLowerLimit();
		btScalar histop = getUpperLimit();
		if(limit && (lostop == histop))
		{  // the joint motor is ineffective
			powered = 0;
		}
		info->m_constraintError[srow] = btScalar(0.0f);
		btScalar currERP = (m_flags & BT_HINGE_FLAGS_ERP_STOP) ? m_stopERP : info->erp;
		if(powered)
		{
			if(m_flags & BT_HINGE_FLAGS_CFM_NORM)
			{
				info->cfm[srow] = m_normalCFM;
			}
			btScalar mot_fact = getMotorFactor(m_hingeAngle, lostop, histop, m_motorTargetVelocity, info->fps * currERP);
			info->m_constraintError[srow] += mot_fact * m_motorTargetVelocity * m_referenceSign;
			info->m_lowerLimit[srow] = - m_maxMotorImpulse;
			info->m_upperLimit[srow] =   m_maxMotorImpulse;
		}
		if(limit)
		{
			k = info->fps * currERP;
			info->m_constraintError[srow] += k * limit_err;
			if(m_flags & BT_HINGE_FLAGS_CFM_STOP)
			{
				info->cfm[srow] = m_stopCFM;
			}
			if(lostop == histop) 
			{
				// limited low and high simultaneously
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			else if(limit == 1) 
			{ // low limit
				info->m_lowerLimit[srow] = 0;
				info->m_upperLimit[srow] = SIMD_INFINITY;
			}
			else 
			{ // high limit
				info->m_lowerLimit[srow] = -SIMD_INFINITY;
				info->m_upperLimit[srow] = 0;
			}
			// bounce (we'll use slider parameter abs(1.0 - m_dampingLimAng) for that)
			btScalar bounce = m_relaxationFactor;
			if(bounce > btScalar(0.0))
			{
				btScalar vel = angVelA.dot(ax1);
				vel -= angVelB.dot(ax1);
				// only apply bounce if the velocity is incoming, and if the
				// resulting c[] exceeds what we already have.
				if(limit == 1)
				{	// low limit
					if(vel < 0)
					{
						btScalar newc = -bounce * vel;
						if(newc > info->m_constraintError[srow])
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
				else
				{	// high limit - all those computations are reversed
					if(vel > 0)
					{
						btScalar newc = -bounce * vel;
						if(newc < info->m_constraintError[srow])
						{
							info->m_constraintError[srow] = newc;
						}
					}
				}
			}
			info->m_constraintError[srow] *= m_biasFactor;
		} // if(limit)
	} // if angular limit or powered
}