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

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

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

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

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

/** get the world tansform of the linked position
 * @param worldTrans out parameter to palce transform in
 * @warning this method is called by physics library and should
 * not be called directly
 */
void MotionState::getWorldTransform(btTransform &worldTrans) const
{
	// set the location/origin
	Point3f& l = this->position->getLocation();
	worldTrans.setOrigin (btVector3(l.getX(), l.getY(), l.getZ()));
	
	// set the basis/rotational matrix
	// since openGL matrix is column major and Bullet is row
	// major, we have to do some converting
	btMatrix3x3 matrix;
	this->position->getRowMajorRotationMatrix(matrix);
	worldTrans.setBasis(matrix);
}

/** get the world tansform of the linked position
 * @param worldTrans out parameter to palce transform in
 * @warning this method is called by physics library and should
 * not be called directly
 */
void MotionState::getWorldTransform(btTransform &worldTrans) const
{
	// set the location/origin
	Vector3 l = this->shape._getTranslation();
    if (this->position != nullptr)
        l += this->position->getLocation();
	worldTrans.setOrigin (btVector3(l.x(), l.y(), l.z()));
	
	// set the basis/rotational matrix
	// since openGL matrix is column major and Bullet is row
	// major, we have to do some converting

    Matrix3 matrix;
    if (this->position != nullptr)
    {
        matrix.setColumn(0, this->position->getLocalXAxis());
        matrix.setColumn(1, this->position->getUpVector());
        matrix.setColumn(2, this->position->getForwardVector());
    }
    matrix.multiply(this->shape._getRotation());


    btMatrix3x3 rot;
    rot.setValue(

        // fill in x axis, first column
        matrix.getColumn(0).x(),
        matrix.getColumn(0).y(),
        matrix.getColumn(0).z(),

        // fill in y axis, second column
        matrix.getColumn(1).x(),
        matrix.getColumn(1).y(),
        matrix.getColumn(1).z(),
                                
        // fill in z axis, thrid column
        matrix.getColumn(2).x(),
        matrix.getColumn(2).y(),
        matrix.getColumn(2).z()
    
    );
	worldTrans.setBasis(rot);
}

void MotionState::getWorldTransform(btTransform& worldTrans) const {
    const Matrix4 transformation = object().transformationMatrix();
    worldTrans.setOrigin(btVector3(transformation.translation()));
    worldTrans.setBasis(btMatrix3x3(transformation.rotationScaling()));
}

void  BulletURDFImporter::getMassAndInertia(int linkIndex, btScalar& mass,btVector3& localInertiaDiagonal, btTransform& inertialFrame) const
{
	//todo(erwincoumans)
	//the link->m_inertia is NOT necessarily aligned with the inertial frame
	//so an additional transform might need to be computed
	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(linkIndex);
	
	
	btAssert(linkPtr);
	if (linkPtr)
	{
		UrdfLink* link = *linkPtr;
		btMatrix3x3 linkInertiaBasis;
		btScalar linkMass, principalInertiaX, principalInertiaY, principalInertiaZ;
		if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
		{
			linkMass = 0.f;
			principalInertiaX = 0.f;
			principalInertiaY = 0.f;
			principalInertiaZ = 0.f;
			linkInertiaBasis.setIdentity();
		}
		else
		{
			linkMass = link->m_inertia.m_mass;
			if (link->m_inertia.m_ixy == 0.0 &&
			    link->m_inertia.m_ixz == 0.0 &&
			    link->m_inertia.m_iyz == 0.0)
			{
				principalInertiaX = link->m_inertia.m_ixx;
				principalInertiaY = link->m_inertia.m_iyy;
				principalInertiaZ = link->m_inertia.m_izz;
				linkInertiaBasis.setIdentity();
			}
			else
			{
				principalInertiaX = link->m_inertia.m_ixx;
				btMatrix3x3 inertiaTensor(link->m_inertia.m_ixx, link->m_inertia.m_ixy, link->m_inertia.m_ixz,
							  link->m_inertia.m_ixy, link->m_inertia.m_iyy, link->m_inertia.m_iyz,
							  link->m_inertia.m_ixz, link->m_inertia.m_iyz, link->m_inertia.m_izz);
				btScalar threshold = 1.0e-6;
				int numIterations = 30;
				inertiaTensor.diagonalize(linkInertiaBasis, threshold, numIterations);
				principalInertiaX = inertiaTensor[0][0];
				principalInertiaY = inertiaTensor[1][1];
				principalInertiaZ = inertiaTensor[2][2];
			}
		}
		mass = linkMass;
		if (principalInertiaX < 0 ||
		    principalInertiaX > (principalInertiaY + principalInertiaZ) ||
		    principalInertiaY < 0 ||
		    principalInertiaY > (principalInertiaX + principalInertiaZ) ||
		    principalInertiaZ < 0 ||
		    principalInertiaZ > (principalInertiaX + principalInertiaY))
		{
			b3Warning("Bad inertia tensor properties, setting inertia to zero for link: %s\n", link->m_name.c_str());
			principalInertiaX = 0.f;
			principalInertiaY = 0.f;
			principalInertiaZ = 0.f;
			linkInertiaBasis.setIdentity();
		}
		localInertiaDiagonal.setValue(principalInertiaX, principalInertiaY, principalInertiaZ);
		inertialFrame.setOrigin(link->m_inertia.m_linkLocalFrame.getOrigin());
		inertialFrame.setBasis(link->m_inertia.m_linkLocalFrame.getBasis()*linkInertiaBasis);
	}
	else
	{
		mass = 1.f;
		localInertiaDiagonal.setValue(1,1,1);
		inertialFrame.setIdentity();
	}
}