当前位置: 首页>>代码示例>>C++>>正文


C++ btSolverBody::applyImpulse方法代码示例

本文整理汇总了C++中btSolverBody::applyImpulse方法的典型用法代码示例。如果您正苦于以下问题:C++ btSolverBody::applyImpulse方法的具体用法?C++ btSolverBody::applyImpulse怎么用?C++ btSolverBody::applyImpulse使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在btSolverBody的用法示例。

在下文中一共展示了btSolverBody::applyImpulse方法的7个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于我们的系统推荐出更棒的C++代码示例。

示例1: resolveSingleConstraintRowGeneric

// Project Gauss Seidel or the equivalent Sequential Impulse
 void btSequentialImpulseConstraintSolver::resolveSingleConstraintRowGeneric(btSolverBody& body1,btSolverBody& body2,const btSolverConstraint& c)
{
	btScalar deltaImpulse = c.m_rhs-btScalar(c.m_appliedImpulse)*c.m_cfm;
	const btScalar deltaVel1Dotn	=	c.m_contactNormal.dot(body1.m_deltaLinearVelocity) 	+ c.m_relpos1CrossNormal.dot(body1.m_deltaAngularVelocity);
	const btScalar deltaVel2Dotn	=	-c.m_contactNormal.dot(body2.m_deltaLinearVelocity) + c.m_relpos2CrossNormal.dot(body2.m_deltaAngularVelocity);

	const btScalar delta_rel_vel	=	deltaVel1Dotn-deltaVel2Dotn;
	deltaImpulse	-=	deltaVel1Dotn*c.m_jacDiagABInv;
	deltaImpulse	-=	deltaVel2Dotn*c.m_jacDiagABInv;

	const btScalar sum = btScalar(c.m_appliedImpulse) + deltaImpulse;
	if (sum < c.m_lowerLimit)
	{
		deltaImpulse = c.m_lowerLimit-c.m_appliedImpulse;
		c.m_appliedImpulse = c.m_lowerLimit;
	}
	else if (sum > c.m_upperLimit) 
	{
		deltaImpulse = c.m_upperLimit-c.m_appliedImpulse;
		c.m_appliedImpulse = c.m_upperLimit;
	}
	else
	{
		c.m_appliedImpulse = sum;
	}
		body1.applyImpulse(c.m_contactNormal*body1.m_invMass,c.m_angularComponentA,deltaImpulse);
		body2.applyImpulse(-c.m_contactNormal*body2.m_invMass,c.m_angularComponentB,deltaImpulse);
}
开发者ID:Geardome,项目名称:ozone-ios,代码行数:29,代码来源:btSequentialImpulseConstraintSolver.cpp


示例2: solveConstraintInt

void btSliderConstraint::solveConstraintInt(btRigidBody& rbA, btSolverBody& bodyA,btRigidBody& rbB, btSolverBody& bodyB)
{
    int i;
    // linear
    btVector3 velA;
	bodyA.getVelocityInLocalPointObsolete(m_relPosA,velA);
    btVector3 velB;
	bodyB.getVelocityInLocalPointObsolete(m_relPosB,velB);
    btVector3 vel = velA - velB;
	for(i = 0; i < 3; i++)
    {
		const btVector3& normal = m_jacLin[i].m_linearJointAxis;
		btScalar rel_vel = normal.dot(vel);
		// calculate positional error
		btScalar depth = m_depth[i];
		// get parameters
		btScalar softness = (i) ? m_softnessOrthoLin : (m_solveLinLim ? m_softnessLimLin : m_softnessDirLin);
		btScalar restitution = (i) ? m_restitutionOrthoLin : (m_solveLinLim ? m_restitutionLimLin : m_restitutionDirLin);
		btScalar damping = (i) ? m_dampingOrthoLin : (m_solveLinLim ? m_dampingLimLin : m_dampingDirLin);
		// calcutate and apply impulse
		btScalar normalImpulse = softness * (restitution * depth / m_timeStep - damping * rel_vel) * m_jacLinDiagABInv[i];
		btVector3 impulse_vector = normal * normalImpulse;
		
		//rbA.applyImpulse( impulse_vector, m_relPosA);
		//rbB.applyImpulse(-impulse_vector, m_relPosB);
		{
			btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
			btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
			bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
			bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
		}



		if(m_poweredLinMotor && (!i))
		{ // apply linear motor
			if(m_accumulatedLinMotorImpulse < m_maxLinMotorForce)
			{
				btScalar desiredMotorVel = m_targetLinMotorVelocity;
				btScalar motor_relvel = desiredMotorVel + rel_vel;
				normalImpulse = -motor_relvel * m_jacLinDiagABInv[i];
				// clamp accumulated impulse
				btScalar new_acc = m_accumulatedLinMotorImpulse + btFabs(normalImpulse);
				if(new_acc  > m_maxLinMotorForce)
				{
					new_acc = m_maxLinMotorForce;
				}
				btScalar del = new_acc  - m_accumulatedLinMotorImpulse;
				if(normalImpulse < btScalar(0.0))
				{
					normalImpulse = -del;
				}
				else
				{
					normalImpulse = del;
				}
				m_accumulatedLinMotorImpulse = new_acc;
				// apply clamped impulse
				impulse_vector = normal * normalImpulse;
				//rbA.applyImpulse( impulse_vector, m_relPosA);
				//rbB.applyImpulse(-impulse_vector, m_relPosB);

				{
					btVector3 ftorqueAxis1 = m_relPosA.cross(normal);
					btVector3 ftorqueAxis2 = m_relPosB.cross(normal);
					bodyA.applyImpulse(normal*rbA.getInvMass(), rbA.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
					bodyB.applyImpulse(normal*rbB.getInvMass(), rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);
				}



			}
		}
    }
	// angular 
	// get axes in world space
	btVector3 axisA =  m_calculatedTransformA.getBasis().getColumn(0);
	btVector3 axisB =  m_calculatedTransformB.getBasis().getColumn(0);

	btVector3 angVelA;
	bodyA.getAngularVelocity(angVelA);
	btVector3 angVelB;
	bodyB.getAngularVelocity(angVelB);

	btVector3 angVelAroundAxisA = axisA * axisA.dot(angVelA);
	btVector3 angVelAroundAxisB = axisB * axisB.dot(angVelB);

	btVector3 angAorthog = angVelA - angVelAroundAxisA;
	btVector3 angBorthog = angVelB - angVelAroundAxisB;
	btVector3 velrelOrthog = angAorthog-angBorthog;
	//solve orthogonal angular velocity correction
	btScalar len = velrelOrthog.length();
	btScalar orthorImpulseMag = 0.f;

	if (len > btScalar(0.00001))
	{
		btVector3 normal = velrelOrthog.normalized();
		btScalar denom = rbA.computeAngularImpulseDenominator(normal) + rbB.computeAngularImpulseDenominator(normal);
		//velrelOrthog *= (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
		orthorImpulseMag = (btScalar(1.)/denom) * m_dampingOrthoAng * m_softnessOrthoAng;
//.........这里部分代码省略.........
开发者ID:jinjoh,项目名称:NOOR,代码行数:101,代码来源:btSliderConstraint.cpp


示例3: angularLimit

void    btHingeConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar    timeStep)
{

    ///for backwards compatibility during the transition to 'getInfo/getInfo2'
    if (m_useSolveConstraintObsolete)
    {

        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();

        btScalar tau = btScalar(0.3);

        //linear part
        if (!m_angularOnly)
        {
            btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
            btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();

            btVector3 vel1,vel2;
            bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
            bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
            btVector3 vel = vel1 - vel2;

            for (int i=0;i<3;i++)
            {
                const btVector3& normal = m_jac[i].m_linearJointAxis;
                btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();

                btScalar rel_vel;
                rel_vel = normal.dot(vel);
                //positional error (zeroth order error)
                btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
                btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
                m_appliedImpulse += impulse;
                btVector3 impulse_vector = normal * impulse;
                btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
                btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
                bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
                bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
            }
        }


        {
            ///solve angular part

            // get axes in world space
            btVector3 axisA =  getRigidBodyA().getCenterOfMassTransform().getBasis() *  m_rbAFrame.getBasis().getColumn(2);
            btVector3 axisB =  getRigidBodyB().getCenterOfMassTransform().getBasis() *  m_rbBFrame.getBasis().getColumn(2);

            btVector3 angVelA;
            bodyA.getAngularVelocity(angVelA);
            btVector3 angVelB;
            bodyB.getAngularVelocity(angVelB);

            btVector3 angVelAroundHingeAxisA = axisA * axisA.dot(angVelA);
            btVector3 angVelAroundHingeAxisB = axisB * axisB.dot(angVelB);

            btVector3 angAorthog = angVelA - angVelAroundHingeAxisA;
            btVector3 angBorthog = angVelB - angVelAroundHingeAxisB;
            btVector3 velrelOrthog = angAorthog-angBorthog;
            {


                //solve orthogonal angular velocity correction
                btScalar len = velrelOrthog.length();
                if (len > btScalar(0.00001))
                {
                    btVector3 normal = velrelOrthog.normalized();
                    btScalar denom = getRigidBodyA().computeAngularImpulseDenominator(normal) +
                        getRigidBodyB().computeAngularImpulseDenominator(normal);
                    // scale for mass and relaxation
                    //velrelOrthog *= (btScalar(1.)/denom) * m_relaxationFactor;

                    bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*velrelOrthog,-(btScalar(1.)/denom));
                    bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*velrelOrthog,(btScalar(1.)/denom));

                }

                //solve angular positional correction
                btVector3 angularError =  axisA.cross(axisB) *(btScalar(1.)/timeStep);
                btScalar len2 = angularError.length();
                if (len2>btScalar(0.00001))
                {
                    btVector3 normal2 = angularError.normalized();
                    btScalar denom2 = getRigidBodyA().computeAngularImpulseDenominator(normal2) +
                            getRigidBodyB().computeAngularImpulseDenominator(normal2);
                    //angularError *= (btScalar(1.)/denom2) * relaxation;

                    bodyA.applyImpulse(btVector3(0,0,0), m_rbA.getInvInertiaTensorWorld()*angularError,(btScalar(1.)/denom2));
                    bodyB.applyImpulse(btVector3(0,0,0), m_rbB.getInvInertiaTensorWorld()*angularError,-(btScalar(1.)/denom2));

                }





                // solve limit
                if (m_solveLimit)
//.........这里部分代码省略.........
开发者ID:gamedevforks,项目名称:mariachi,代码行数:101,代码来源:btHingeConstraint.cpp


示例4: normal

void    btPoint2PointConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar    timeStep)
{
    if (m_useSolveConstraintObsolete)
    {
        btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_pivotInA;
        btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_pivotInB;


        btVector3 normal(0,0,0);


    //    btVector3 angvelA = m_rbA.getCenterOfMassTransform().getBasis().transpose() * m_rbA.getAngularVelocity();
    //    btVector3 angvelB = m_rbB.getCenterOfMassTransform().getBasis().transpose() * m_rbB.getAngularVelocity();

        for (int i=0;i<3;i++)
        {
            normal[i] = 1;
            btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();

            btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition();
            btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();
            //this jacobian entry could be re-used for all iterations

            btVector3 vel1,vel2;
            bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
            bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
            btVector3 vel = vel1 - vel2;

            btScalar rel_vel;
            rel_vel = normal.dot(vel);

        /*
            //velocity error (first order error)
            btScalar rel_vel = m_jac[i].getRelativeVelocity(m_rbA.getLinearVelocity(),angvelA,
                                                            m_rbB.getLinearVelocity(),angvelB);
        */

            //positional error (zeroth order error)
            btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal

            btScalar deltaImpulse = depth*m_setting.m_tau/timeStep  * jacDiagABInv -  m_setting.m_damping * rel_vel * jacDiagABInv;

            btScalar impulseClamp = m_setting.m_impulseClamp;

            const btScalar sum = btScalar(m_appliedImpulse) + deltaImpulse;
            if (sum < -impulseClamp)
            {
                deltaImpulse = -impulseClamp-m_appliedImpulse;
                m_appliedImpulse = -impulseClamp;
            }
            else if (sum > impulseClamp)
            {
                deltaImpulse = impulseClamp-m_appliedImpulse;
                m_appliedImpulse = impulseClamp;
            }
            else
            {
                m_appliedImpulse = sum;
            }


            btVector3 impulse_vector = normal * deltaImpulse;

            btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
            btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
            bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,deltaImpulse);
            bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-deltaImpulse);


            normal[i] = 0;
        }
    }
}
开发者ID:gamedevforks,项目名称:mariachi,代码行数:73,代码来源:btPoint2PointConstraint.cpp


示例5: zerovec

void	btConeTwistConstraint::solveConstraintObsolete(btSolverBody& bodyA,btSolverBody& bodyB,btScalar	timeStep)
{
	if (m_useSolveConstraintObsolete)
	{
		btVector3 pivotAInW = m_rbA.getCenterOfMassTransform()*m_rbAFrame.getOrigin();
		btVector3 pivotBInW = m_rbB.getCenterOfMassTransform()*m_rbBFrame.getOrigin();

		btScalar tau = btScalar(0.3);

		//linear part
		if (!m_angularOnly)
		{
			btVector3 rel_pos1 = pivotAInW - m_rbA.getCenterOfMassPosition(); 
			btVector3 rel_pos2 = pivotBInW - m_rbB.getCenterOfMassPosition();

			btVector3 vel1;
			bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
			btVector3 vel2;
			bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
			btVector3 vel = vel1 - vel2;

			for (int i=0;i<3;i++)
			{		
				const btVector3& normal = m_jac[i].m_linearJointAxis;
				btScalar jacDiagABInv = btScalar(1.) / m_jac[i].getDiagonal();

				btScalar rel_vel;
				rel_vel = normal.dot(vel);
				//positional error (zeroth order error)
				btScalar depth = -(pivotAInW - pivotBInW).dot(normal); //this is the error projected on the normal
				btScalar impulse = depth*tau/timeStep  * jacDiagABInv -  rel_vel * jacDiagABInv;
				m_appliedImpulse += impulse;
				
				btVector3 ftorqueAxis1 = rel_pos1.cross(normal);
				btVector3 ftorqueAxis2 = rel_pos2.cross(normal);
				bodyA.applyImpulse(normal*m_rbA.getInvMass(), m_rbA.getInvInertiaTensorWorld()*ftorqueAxis1,impulse);
				bodyB.applyImpulse(normal*m_rbB.getInvMass(), m_rbB.getInvInertiaTensorWorld()*ftorqueAxis2,-impulse);
		
			}
		}

		// apply motor
		if (m_bMotorEnabled)
		{
			// compute current and predicted transforms
			btTransform trACur = m_rbA.getCenterOfMassTransform();
			btTransform trBCur = m_rbB.getCenterOfMassTransform();
			btVector3 omegaA; bodyA.getAngularVelocity(omegaA);
			btVector3 omegaB; bodyB.getAngularVelocity(omegaB);
			btTransform trAPred; trAPred.setIdentity(); 
			btVector3 zerovec(0,0,0);
			btTransformUtil::integrateTransform(
				trACur, zerovec, omegaA, timeStep, trAPred);
			btTransform trBPred; trBPred.setIdentity(); 
			btTransformUtil::integrateTransform(
				trBCur, zerovec, omegaB, timeStep, trBPred);

			// compute desired transforms in world
			btTransform trPose(m_qTarget);
			btTransform trABDes = m_rbBFrame * trPose * m_rbAFrame.inverse();
			btTransform trADes = trBPred * trABDes;
			btTransform trBDes = trAPred * trABDes.inverse();

			// compute desired omegas in world
			btVector3 omegaADes, omegaBDes;
			
			btTransformUtil::calculateVelocity(trACur, trADes, timeStep, zerovec, omegaADes);
			btTransformUtil::calculateVelocity(trBCur, trBDes, timeStep, zerovec, omegaBDes);

			// compute delta omegas
			btVector3 dOmegaA = omegaADes - omegaA;
			btVector3 dOmegaB = omegaBDes - omegaB;

			// compute weighted avg axis of dOmega (weighting based on inertias)
			btVector3 axisA, axisB;
			btScalar kAxisAInv = 0, kAxisBInv = 0;

			if (dOmegaA.length2() > SIMD_EPSILON)
			{
				axisA = dOmegaA.normalized();
				kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(axisA);
			}

			if (dOmegaB.length2() > SIMD_EPSILON)
			{
				axisB = dOmegaB.normalized();
				kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(axisB);
			}

			btVector3 avgAxis = kAxisAInv * axisA + kAxisBInv * axisB;

			static bool bDoTorque = true;
			if (bDoTorque && avgAxis.length2() > SIMD_EPSILON)
			{
				avgAxis.normalize();
				kAxisAInv = getRigidBodyA().computeAngularImpulseDenominator(avgAxis);
				kAxisBInv = getRigidBodyB().computeAngularImpulseDenominator(avgAxis);
				btScalar kInvCombined = kAxisAInv + kAxisBInv;

				btVector3 impulse = (kAxisAInv * dOmegaA - kAxisBInv * dOmegaB) /
//.........这里部分代码省略.........
开发者ID:Geardome,项目名称:ozone-ios,代码行数:101,代码来源:btConeTwistConstraint.cpp


示例6: solveLinearAxis

btScalar btTranslationalLimitMotor::solveLinearAxis(
	btScalar timeStep,
	btScalar jacDiagABInv,
	btRigidBody& body1,btSolverBody& bodyA,const btVector3 &pointInA,
	btRigidBody& body2,btSolverBody& bodyB,const btVector3 &pointInB,
	int limit_index,
	const btVector3 & axis_normal_on_a,
	const btVector3 & anchorPos)
{

	///find relative velocity
	//    btVector3 rel_pos1 = pointInA - body1.getCenterOfMassPosition();
	//    btVector3 rel_pos2 = pointInB - body2.getCenterOfMassPosition();
	btVector3 rel_pos1 = anchorPos - body1.getCenterOfMassPosition();
	btVector3 rel_pos2 = anchorPos - body2.getCenterOfMassPosition();

	btVector3 vel1;
	bodyA.getVelocityInLocalPointObsolete(rel_pos1,vel1);
	btVector3 vel2;
	bodyB.getVelocityInLocalPointObsolete(rel_pos2,vel2);
	btVector3 vel = vel1 - vel2;

	btScalar rel_vel = axis_normal_on_a.dot(vel);



	/// apply displacement correction

	//positional error (zeroth order error)
	btScalar depth = -(pointInA - pointInB).dot(axis_normal_on_a);
	btScalar	lo = btScalar(-1e30);
	btScalar	hi = btScalar(1e30);

	btScalar minLimit = m_lowerLimit[limit_index];
	btScalar maxLimit = m_upperLimit[limit_index];

	//handle the limits
	if (minLimit < maxLimit)
	{
		{
			if (depth > maxLimit)
			{
				depth -= maxLimit;
				lo = btScalar(0.);

			}
			else
			{
				if (depth < minLimit)
				{
					depth -= minLimit;
					hi = btScalar(0.);
				}
				else
				{
					return 0.0f;
				}
			}
		}
	}

	btScalar normalImpulse= m_limitSoftness*(m_restitution*depth/timeStep - m_damping*rel_vel) * jacDiagABInv;




	btScalar oldNormalImpulse = m_accumulatedImpulse[limit_index];
	btScalar sum = oldNormalImpulse + normalImpulse;
	m_accumulatedImpulse[limit_index] = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;
	normalImpulse = m_accumulatedImpulse[limit_index] - oldNormalImpulse;

	btVector3 impulse_vector = axis_normal_on_a * normalImpulse;
	//body1.applyImpulse( impulse_vector, rel_pos1);
	//body2.applyImpulse(-impulse_vector, rel_pos2);

	btVector3 ftorqueAxis1 = rel_pos1.cross(axis_normal_on_a);
	btVector3 ftorqueAxis2 = rel_pos2.cross(axis_normal_on_a);
	bodyA.applyImpulse(axis_normal_on_a*body1.getInvMass(), body1.getInvInertiaTensorWorld()*ftorqueAxis1,normalImpulse);
	bodyB.applyImpulse(axis_normal_on_a*body2.getInvMass(), body2.getInvInertiaTensorWorld()*ftorqueAxis2,-normalImpulse);




	return normalImpulse;
}
开发者ID:CZdravko,项目名称:Horde,代码行数:85,代码来源:btGeneric6DofConstraint.cpp


示例7: solveAngularLimits

btScalar btRotationalLimitMotor::solveAngularLimits(
	btScalar timeStep,btVector3& axis,btScalar jacDiagABInv,
	btRigidBody * body0, btSolverBody& bodyA, btRigidBody * body1, btSolverBody& bodyB)
{
	if (needApplyTorques()==false) return 0.0f;

	btScalar target_velocity = m_targetVelocity;
	btScalar maxMotorForce = m_maxMotorForce;

	//current error correction
	if (m_currentLimit!=0)
	{
		target_velocity = -m_ERP*m_currentLimitError/(timeStep);
		maxMotorForce = m_maxLimitForce;
	}

	maxMotorForce *= timeStep;

	// current velocity difference

	btVector3 angVelA;
	bodyA.getAngularVelocity(angVelA);
	btVector3 angVelB;
	bodyB.getAngularVelocity(angVelB);

	btVector3 vel_diff;
	vel_diff = angVelA-angVelB;



	btScalar rel_vel = axis.dot(vel_diff);

	// correction velocity
	btScalar motor_relvel = m_limitSoftness*(target_velocity  - m_damping*rel_vel);


	if ( motor_relvel < SIMD_EPSILON && motor_relvel > -SIMD_EPSILON  )
	{
		return 0.0f;//no need for applying force
	}


	// correction impulse
	btScalar unclippedMotorImpulse = (1+m_bounce)*motor_relvel*jacDiagABInv;

	// clip correction impulse
	btScalar clippedMotorImpulse;

	///@todo: should clip against accumulated impulse
	if (unclippedMotorImpulse>0.0f)
	{
		clippedMotorImpulse =  unclippedMotorImpulse > maxMotorForce? maxMotorForce: unclippedMotorImpulse;
	}
	else
	{
		clippedMotorImpulse =  unclippedMotorImpulse < -maxMotorForce ? -maxMotorForce: unclippedMotorImpulse;
	}


	// sort with accumulated impulses
	btScalar	lo = btScalar(-1e30);
	btScalar	hi = btScalar(1e30);

	btScalar oldaccumImpulse = m_accumulatedImpulse;
	btScalar sum = oldaccumImpulse + clippedMotorImpulse;
	m_accumulatedImpulse = sum > hi ? btScalar(0.) : sum < lo ? btScalar(0.) : sum;

	clippedMotorImpulse = m_accumulatedImpulse - oldaccumImpulse;

	btVector3 motorImp = clippedMotorImpulse * axis;

	//body0->applyTorqueImpulse(motorImp);
	//body1->applyTorqueImpulse(-motorImp);

	bodyA.applyImpulse(btVector3(0,0,0), body0->getInvInertiaTensorWorld()*axis,clippedMotorImpulse);
	bodyB.applyImpulse(btVector3(0,0,0), body1->getInvInertiaTensorWorld()*axis,-clippedMotorImpulse);


	return clippedMotorImpulse;


}
开发者ID:CZdravko,项目名称:Horde,代码行数:82,代码来源:btGeneric6DofConstraint.cpp



注:本文中的btSolverBody::applyImpulse方法示例整理自Github/MSDocs等源码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。