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

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

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

示例1: 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

示例2: 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

示例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: 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::getAngularVelocity方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台，相关代码片段筛选自各路编程大神贡献的开源项目，源码版权归原作者所有，传播和使用请参考对应项目的License；未经允许，请勿转载。