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

	btSoftSingleRayCallback(const btVector3& rayFromWorld,const btVector3& rayToWorld,const btSoftRigidDynamicsWorld* world,btCollisionWorld::RayResultCallback& resultCallback)
	:m_rayFromWorld(rayFromWorld),
	m_rayToWorld(rayToWorld),
	m_world(world),
	m_resultCallback(resultCallback)
	{
		m_rayFromTrans.setIdentity();
		m_rayFromTrans.setOrigin(m_rayFromWorld);
		m_rayToTrans.setIdentity();
		m_rayToTrans.setOrigin(m_rayToWorld);

		btVector3 rayDir = (rayToWorld-rayFromWorld);

		rayDir.normalize ();
		///what about division by zero? --> just set rayDirection[i] to INF/1e30
		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
		m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
		m_signs[0] = m_rayDirectionInverse[0] < 0.0;
		m_signs[1] = m_rayDirectionInverse[1] < 0.0;
		m_signs[2] = m_rayDirectionInverse[2] < 0.0;

		m_lambda_max = rayDir.dot(m_rayToWorld-m_rayFromWorld);

	}

void ConvertMatrixToBull(const matrix3x4_t& hl, btTransform& transform)
{
	Vector forward, left, up, pos;

	forward.x = hl[0][0];
	forward.y = hl[1][0];
	forward.z = hl[2][0];

	left.x = hl[0][1];
	left.y = hl[1][1];
	left.z = hl[2][1];

	up.x = hl[0][2];
	up.y = hl[1][2];
	up.z = hl[2][2];

	pos.x = hl[0][3];
	pos.y = hl[1][3];
	pos.z = hl[2][3];

	btVector3 bullForward, bullLeft, bullUp, origin;
	ConvertDirectionToBull(forward, bullForward);
	ConvertDirectionToBull(-left, bullLeft);
	ConvertDirectionToBull(up, bullUp);
	ConvertPosToBull(pos, origin);

	transform.setBasis(btMatrix3x3(bullForward.x(), bullUp.x(), bullLeft.x(), bullForward.y(), bullUp.y(), bullLeft.y(), bullForward.z(), bullUp.z(), bullLeft.z()));
	transform.setOrigin(origin);
}

//**************************************************************************************************************************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);
}

// 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()));
}

//-------------------------------------------------------------------------------------------------------
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 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);
}

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

// 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	addCamera(_bObj* tmpObject)
	{
		m_cameraTrans.setOrigin(btVector3(tmpObject->location[IRR_X],tmpObject->location[IRR_Y],tmpObject->location[IRR_Z]));
		btMatrix3x3 mat;
		mat.setEulerZYX(tmpObject->rotphr[0],tmpObject->rotphr[1],tmpObject->rotphr[2]);
		m_cameraTrans.setBasis(mat);
	}

void btCompoundShape::calculatePrincipalAxisTransform(btScalar* masses, btTransform& principal, btVector3& inertia) const
{
	int n = m_children.size();

	btScalar totalMass = 0;
	btVector3 center(0, 0, 0);
	int k;

	for (k = 0; k < n; k++)
	{
		btAssert(masses[k]>0);
		center += m_children[k].m_transform.getOrigin() * masses[k];
		totalMass += masses[k];
	}

	btAssert(totalMass>0);

	center /= totalMass;
	principal.setOrigin(center);

	btMatrix3x3 tensor(0, 0, 0, 0, 0, 0, 0, 0, 0);
	for ( k = 0; k < n; k++)
	{
		btVector3 i;
		m_children[k].m_childShape->calculateLocalInertia(masses[k], i);

		const btTransform& t = m_children[k].m_transform;
		btVector3 o = t.getOrigin() - center;

		//compute inertia tensor in coordinate system of compound shape
		btMatrix3x3 j = t.getBasis().transpose();
		j[0] *= i[0];
		j[1] *= i[1];
		j[2] *= i[2];
		j = t.getBasis() * j;

		//add inertia tensor
		tensor[0] += j[0];
		tensor[1] += j[1];
		tensor[2] += j[2];

		//compute inertia tensor of pointmass at o
		btScalar o2 = o.length2();
		j[0].setValue(o2, 0, 0);
		j[1].setValue(0, o2, 0);
		j[2].setValue(0, 0, o2);
		j[0] += o * -o.x(); 
		j[1] += o * -o.y(); 
		j[2] += o * -o.z();

		//add inertia tensor of pointmass
		tensor[0] += masses[k] * j[0];
		tensor[1] += masses[k] * j[1];
		tensor[2] += masses[k] * j[2];
	}

	tensor.diagonalize(principal.getBasis(), btScalar(0.00001), 20);
	inertia.setValue(tensor[0][0], tensor[1][1], tensor[2][2]);
}

static void draw(void)
{
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);

    float xpos = box1->getCenterOfMassPosition().getX();
    float ypos = box1->getCenterOfMassPosition().getY();
    float zpos = box1->getCenterOfMassPosition().getZ();

    printf("%f %f %f\n",xpos,ypos,zpos);
    
	btTransform trans = box1->getWorldTransform();
	trans.setOrigin(btVector3(BoxVel[0], BoxVel[1], BoxVel[2]));
	box1->setWorldTransform(trans);
	box1->getMotionState()->getWorldTransform(trans);
	trans.setOrigin(btVector3(BoxVel[0], BoxVel[1], BoxVel[2]));
	box1->getMotionState()->setWorldTransform(trans);

//*** draw box1 
glColor3f(0.0, 0.0, 1.0);
glPushMatrix();
box1->getMotionState()->getWorldTransform(trans);
trans.getOpenGLMatrix(matrix);
glMultMatrixf(matrix);
glutSolidCube(40);
glPopMatrix();

//*** draw box2
glColor3f(1.0, 1.0, 0.0);
glPushMatrix();
box2->getMotionState()->getWorldTransform(trans);
trans.getOpenGLMatrix(matrix);
glMultMatrixf(matrix);
glutSolidCube(10);
glPopMatrix();

// draw box 3
glColor3f(0.0,0.0,1.0);
glPushMatrix();
box3->getMotionState()->getWorldTransform(trans);
trans.getOpenGLMatrix(matrix);
glMultMatrixf(matrix);
glutSolidCube(30);
glPopMatrix();

glutSwapBuffers();
}

	void getWorldTransform(btTransform& worldTrans) const
    {
        worldTrans.setBasis( btMatrix3x3(transform[0][0], transform[0][1], transform[0][2],
                                         transform[1][0], transform[1][1], transform[1][2],
                                         transform[2][0], transform[2][1], transform[2][2]) );

        worldTrans.setOrigin( btVector3(transform[0][3], transform[1][3], transform[2][3]));
    }

	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 gaen_to_bullet_transform(btTransform & bT, const mat43 & gT)
{
    bT.setBasis(btMatrix3x3(gT[0][0], gT[1][0], gT[2][0],
                            gT[0][1], gT[1][1], gT[2][1],
                            gT[0][2], gT[1][2], gT[2][2]));

    bT.setOrigin(btVector3(gT[3][0], gT[3][1], gT[3][2]));
}

// 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()));
    }
}