C++ btTransform::setRotation方法代码示例

void getUnitMeterScalingAndUpAxisTransform(TiXmlDocument& doc, btTransform& tr, float& unitMeterScaling)
{
	TiXmlElement* unitMeter = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("unit");
	if (unitMeter)
	{
		const char* meterText = unitMeter->Attribute("meter");
		printf("meterText=%s\n", meterText);
		unitMeterScaling = atof(meterText);
	}

	TiXmlElement* upAxisElem = doc.RootElement()->FirstChildElement("asset")->FirstChildElement("up_axis");
	if (upAxisElem)
	{
		std::string upAxisTxt = upAxisElem->GetText();
		if (upAxisTxt == "X_UP")
		{
			btQuaternion y2x(btVector3(0,0,1),SIMD_HALF_PI);
			tr.setRotation(y2x);
		}
		if (upAxisTxt == "Y_UP")
		{
			//assume Y_UP for now, to be compatible with assimp?
		}
		if (upAxisTxt == "Z_UP")
		{
			btQuaternion y2z(btVector3(1,0,0),-SIMD_HALF_PI);
			tr.setRotation(y2z);
		}
	}
}

//**************************************************************************************************************************pose2DtoTf()
void PSMpositionNode::pose2DToTf(const geometry_msgs::Pose2D& pose, btTransform& t)
{
    t.setOrigin(btVector3(pose.x, pose.y, 0.0));
    btQuaternion q;
    q.setRPY(0, 0, pose.theta);
    t.setRotation(q);
}

//-------------------------------------------------------------------------------------------------------
void Road::updateTriggerPosition(btTransform& trans)
{
    trans.setIdentity();
    trans.setOrigin(GameState::ogreVecToBullet(mPosition));
    Ogre::Quaternion rotation = Ogre::Vector3::UNIT_Z.getRotationTo(mDirection);
    trans.setRotation(btQuaternion(rotation.x, rotation.y, rotation.z, rotation.w));
}

void	fbMotionState::getWorldTransform(btTransform& worldTrans) const
{
	assert(mVisualObj);
	worldTrans.setIdentity();
	worldTrans.setRotation(FBToBullet(mVisualObj->GetRot()));
	worldTrans.setOrigin(FBToBullet(mVisualObj->GetPos()));	
}

void MotionState::getWorldTransform(btTransform &transform) const
{
    lua_rawgeti(L, LUA_REGISTRYINDEX, component_ref);
    lua_getfield(L, -1, "transform");
    lua_remove(L, -2);
    // the stack now contains just the transform component
    
    // get the origin
    {
        lua_getfield(L, -1, "pos");
        double x, y, z;
        l_tovect(L, -1, &x, &y, &z);
        lua_pop(L, 1);

        transform.setOrigin(btVector3(x, y, z));
    }

    // get the rotation
    {
        lua_getfield(L, -1, "orientation");
        double w, x, y, z;
        l_toquaternion(L, -1, &w, &x, &y, &z);
        lua_pop(L, 1);

        transform.setRotation(btQuaternion(x, y, z, w));
    }

    lua_pop(L, 1);
}

// This callback is invoked by the physics simulation in two cases:
// (1) when the RigidBody is first added to the world
//     (irregardless of PhysicsMotionType: STATIC, DYNAMIC, or KINEMATIC)
// (2) at the beginning of each simulation step for KINEMATIC RigidBody's --
//     it is an opportunity for outside code to update the object's simulation position
void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
    BT_PROFILE("getWorldTransform");
    if (!_entity) {
        return;
    }
    assert(entityTreeIsLocked());
    if (_motionType == MOTION_TYPE_KINEMATIC) {
        BT_PROFILE("kinematicIntegration");
        uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
        if (hasInternalKinematicChanges()) {
            // ACTION_CAN_CONTROL_KINEMATIC_OBJECT_HACK: This kinematic body was moved by an Action
            // and doesn't require transform update because the body is authoritative and its transform
            // has already been copied out --> do no kinematic integration.
            clearInternalKinematicChanges();
            _lastKinematicStep = thisStep;
            return;
        }
        // This is physical kinematic motion which steps strictly by the subframe count
        // of the physics simulation and uses full gravity for acceleration.
        _entity->setAcceleration(_entity->getGravity());

        float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
        _lastKinematicStep = thisStep;
        _entity->stepKinematicMotion(dt);

        // and set the acceleration-matches-gravity count high so that if we send an update
        // it will use the correct acceleration for remote simulations
        _accelerationNearlyGravityCount = (uint8_t)(-1);
    }
    worldTrans.setOrigin(glmToBullet(getObjectPosition()));
    worldTrans.setRotation(glmToBullet(_entity->getWorldOrientation()));
}

// This callback is invoked by the physics simulation in two cases:
// (1) when the RigidBody is first added to the world
//     (irregardless of PhysicsMotionType: STATIC, DYNAMIC, or KINEMATIC)
// (2) at the beginning of each simulation step for KINEMATIC RigidBody's --
//     it is an opportunity for outside code to update the object's simulation position
void EntityMotionState::getWorldTransform(btTransform& worldTrans) const {
    BT_PROFILE("getWorldTransform");
    if (!_entity) {
        return;
    }
    assert(entityTreeIsLocked());
    if (_motionType == MOTION_TYPE_KINEMATIC) {
        BT_PROFILE("kinematicIntegration");
        // This is physical kinematic motion which steps strictly by the subframe count
        // of the physics simulation and uses full gravity for acceleration.
        _entity->setAcceleration(_entity->getGravity());
        uint32_t thisStep = ObjectMotionState::getWorldSimulationStep();
        float dt = (thisStep - _lastKinematicStep) * PHYSICS_ENGINE_FIXED_SUBSTEP;
        _entity->stepKinematicMotion(dt);

        // bypass const-ness so we can remember the step
        const_cast<EntityMotionState*>(this)->_lastKinematicStep = thisStep;

        // and set the acceleration-matches-gravity count high so that if we send an update
        // it will use the correct acceleration for remote simulations
        _accelerationNearlyGravityCount = (uint8_t)(-1);
    }
    worldTrans.setOrigin(glmToBullet(getObjectPosition()));
    worldTrans.setRotation(glmToBullet(_entity->getRotation()));
}

// virtual
void AvatarMotionState::getWorldTransform(btTransform& worldTrans) const {
    worldTrans.setOrigin(glmToBullet(getObjectPosition()));
    worldTrans.setRotation(glmToBullet(getObjectRotation()));
    if (_body) {
        _body->setLinearVelocity(glmToBullet(getObjectLinearVelocity()));
        _body->setAngularVelocity(glmToBullet(getObjectAngularVelocity()));
    }
}

	virtual void getWorldTransform(btTransform& tform) const
	{
		const auto& p = m_part->dummy->getDefaultTranslation();
		const auto& o = glm::toQuat(m_part->dummy->getDefaultRotation());
		tform.setOrigin(btVector3(p.x, p.y, p.z));
		tform.setRotation(btQuaternion(o.x, o.y, o.z, o.w));
		tform = m_object->collision->getBulletBody()->getWorldTransform() * tform;
	}

void EntityMotionState::getWorldTransform(btTransform& transform) const {
    if(!owner) {
	transform = original_transform;
	return;
    }

    const float* position = owner->getPosition();
    transform.setOrigin(btVector3(position[0], position[1], position[2]));
    const float* rotation = owner->getQuaternionRotation();
    transform.setRotation(btQuaternion(rotation[0], rotation[1], rotation[2], rotation[3]));
}

    /// btMotionState override. Called when Bullet wants us to tell the body's initial transform
    void getWorldTransform(btTransform &worldTrans) const
    {
        if (placeable.Expired())
            return;

        float3 position = placeable->WorldPosition();
        Quat orientation = placeable->WorldOrientation();
    
        worldTrans.setOrigin(position);
        worldTrans.setRotation(orientation);
    }

 void DiBone::GetOffsetTransform(btTransform& t) const
 {
     DiVec3 locScale = GetDerivedScale() * mBindDerivedInverseScale;
     DiQuat locRotate = GetDerivedOrientation() * mBindDerivedInverseOrientation;
     DiVec3 locTranslate = GetDerivedPosition() +
     locRotate * (locScale * mBindDerivedInversePosition);
     t.setOrigin(btVector3(locTranslate.x,locTranslate.y,locTranslate.z));
     btQuaternion qt;
     qt.setValue(locRotate.x,locRotate.y,locRotate.z,locRotate.w);
     t.setRotation(qt);
     t.setBasis(t.getBasis().scaled(btVector3(locScale.x, locScale.y, locScale.z)));
 }

void RigidBody::getWorldTransform(btTransform& worldTrans) const
{
    // We may be in a pathological state where a RigidBody exists without a scene node when this callback is fired,
    // so check to be sure
    if (node_)
    {
        lastPosition_ = node_->GetWorldPosition();
        lastRotation_ = node_->GetWorldRotation();
        worldTrans.setOrigin(ToBtVector3(lastPosition_ + lastRotation_ * centerOfMass_));
        worldTrans.setRotation(ToBtQuaternion(lastRotation_));
    }
}

bool ROSURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit) const
{
    jointLowerLimit = 0.f;
    jointUpperLimit = 0.f;
        
    if ((*m_data->m_links[urdfLinkIndex]).parent_joint)
    {
        my_shared_ptr<Joint> pj =(*m_data->m_links[urdfLinkIndex]).parent_joint;
            
        const urdf::Vector3 pos = pj->parent_to_joint_origin_transform.position;
        const urdf::Rotation orn = pj->parent_to_joint_origin_transform.rotation;
            
        jointAxisInJointSpace.setValue(pj->axis.x,pj->axis.y,pj->axis.z);
        parent2joint.setOrigin(btVector3(pos.x,pos.y,pos.z));
        parent2joint.setRotation(btQuaternion(orn.x,orn.y,orn.z,orn.w));

        switch (pj->type)
        {
            case Joint::REVOLUTE:
                jointType = URDFRevoluteJoint;
                break;
            case Joint::FIXED:
                jointType = URDFFixedJoint;
                break;
            case Joint::PRISMATIC:
                jointType = URDFPrismaticJoint;
                break;
            case Joint::PLANAR:
                jointType = URDFPlanarJoint;
                break;
            case Joint::CONTINUOUS:
				jointType = URDFContinuousJoint;
                break;
            default:
            {
                printf("Error: unknown joint type %d\n", pj->type);
                btAssert(0);
            }
                    
        };
            
        if (pj->limits)
        {
            jointLowerLimit = pj->limits.get()->lower;
            jointUpperLimit = pj->limits.get()->upper;
        }
        return true;
    } else
    {
        parent2joint.setIdentity();
        return false;
    }
}

void  ROSURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
    if ((*m_data->m_links[linkIndex]).inertial)
    {
        mass = (*m_data->m_links[linkIndex]).inertial->mass;
        localInertiaDiagonal.setValue((*m_data->m_links[linkIndex]).inertial->ixx,(*m_data->m_links[linkIndex]).inertial->iyy,(*m_data->m_links[linkIndex]).inertial->izz);
        inertialFrame.setOrigin(btVector3((*m_data->m_links[linkIndex]).inertial->origin.position.x,(*m_data->m_links[linkIndex]).inertial->origin.position.y,(*m_data->m_links[linkIndex]).inertial->origin.position.z));
        inertialFrame.setRotation(btQuaternion((*m_data->m_links[linkIndex]).inertial->origin.rotation.x,(*m_data->m_links[linkIndex]).inertial->origin.rotation.y,(*m_data->m_links[linkIndex]).inertial->origin.rotation.z,(*m_data->m_links[linkIndex]).inertial->origin.rotation.w));
    } else
    {
        mass = 1.f;
        localInertiaDiagonal.setValue(1,1,1);
        inertialFrame.setIdentity();
    }
}