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

void	btCollisionWorld::rayTest(const btVector3& rayFromWorld, const btVector3& rayToWorld, RayResultCallback& resultCallback,short int collisionFilterMask)
{

	
	btTransform	rayFromTrans,rayToTrans;
	rayFromTrans.setIdentity();
	rayFromTrans.setOrigin(rayFromWorld);
	rayToTrans.setIdentity();
	
	rayToTrans.setOrigin(rayToWorld);

	/// go over all objects, and if the ray intersects their aabb, do a ray-shape query using convexCaster (CCD)
	
	int i;
	for (i=0;i<m_collisionObjects.size();i++)
	{
		btCollisionObject*	collisionObject= m_collisionObjects[i];
		//only perform raycast if filterMask matches
		if(collisionObject->getBroadphaseHandle()->m_collisionFilterGroup & collisionFilterMask) { 
			//RigidcollisionObject* collisionObject = ctrl->GetRigidcollisionObject();
			btVector3 collisionObjectAabbMin,collisionObjectAabbMax;
			collisionObject->getCollisionShape()->getAabb(collisionObject->getWorldTransform(),collisionObjectAabbMin,collisionObjectAabbMax);

			btScalar hitLambda = btScalar(1.); //could use resultCallback.m_closestHitFraction, but needs testing
			btVector3 hitNormal;
			if (btRayAabb(rayFromWorld,rayToWorld,collisionObjectAabbMin,collisionObjectAabbMax,hitLambda,hitNormal))
			{
				rayTestSingle(rayFromTrans,rayToTrans,
					collisionObject,
						collisionObject->getCollisionShape(),
						collisionObject->getWorldTransform(),
						resultCallback);
			}	
		}
	}

}

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

    //-----------------------------------------------------------------------
    //                   g e t W o r l d T r a n s f o r m
    //-----------------------------------------------------------------------
    // getWorldTransform is invoked when any body is initially created. After
    // that, it is invoked only for kinematic objects.
    void TPhysicsObject::getWorldTransform(btTransform& centerOfMassWorldTrans) const
    {
        if(!m_sceneNode)
            return;

        m_sceneNode->updateAbsolutePosition();
        /*
        TVector3 pos,rot(0,0,0);
        pos = m_sceneNode->getAbsolutePosition();
        TMatrix4::getRotationDegreesDivScale(m_sceneNode->getAbsoluteTransformation(), rot);
        TIBConvert::IrrToBullet(pos, rot, centerOfMassWorldTrans);
        */
        centerOfMassWorldTrans.setIdentity();
        centerOfMassWorldTrans.setFromOpenGLMatrix(m_sceneNode->getAbsoluteTransformation().pointer()); 
    }

    void body(int bodytype, vec3 origin,vec3 size, vec3 inertia,float mass) {
        btCollisionShape* aShape = NULL;
		pos[0] = origin.x; pos[1] = origin.y; pos[2] = origin.z;
        inertia.x = 1;
		inertia.y = 1;
		inertia.z = 1;
		mass = 1;

		switch (bodytype) {
            case BODYTYPE_BOX:
                aShape = new btBoxShape(btVector3(btScalar(size.x),btScalar(size.y),btScalar(size.z)));
                break;
			case BODYTYPE_CYLINDER:
				aShape = new btCylinderShape(btVector3(btScalar(size.x),btScalar(size.y),btScalar(size.z)));
                break;
			case BODYTYPE_SPHERE:
				aShape = new btSphereShape(btScalar(size.x));
		        break;
        	case BODYTYPE_CAPSULE:
				aShape = new btCapsuleShape(btScalar(size.x),btScalar(size.y));
                break;
			case BODYTYPE_CONE:
				aShape = new btConeShape(btScalar(size.x),btScalar(size.y));
		        break;
        }
        
        if (aShape) {
            btScalar Mass(mass);
            bool isDynamic = (Mass != 0.f);
            btVector3 localInertia(inertia.x,inertia.y,inertia.z);
            if (isDynamic)
                aShape->calculateLocalInertia(mass,localInertia);
            transform.setIdentity();
            transform.setOrigin(btVector3(origin.x,origin.y,origin.z));
			btDefaultMotionState* myMotionState = new btDefaultMotionState(transform);
			btRigidBody::btRigidBodyConstructionInfo rbInfo(Mass,myMotionState,aShape,localInertia);
			body1 = new btRigidBody(rbInfo);            
            collisionShapes.push_back(aShape);
			dynamicsWorld->addRigidBody(body1);
        }

        //return body1;
    }

bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger)
{
	tr.setIdentity();
	
	{
		const char* xyz_str = xml->Attribute("xyz");
		if (xyz_str)
		{
			parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
		}
	}
	
	{
		const char* rpy_str = xml->Attribute("rpy");
		if (rpy_str != NULL)
		{
			btVector3 rpy;
			if (parseVector3(rpy,std::string(rpy_str),logger))
			{
				double phi, the, psi;
				double roll = rpy[0];
				double pitch = rpy[1];
				double yaw = rpy[2];
				
				phi = roll / 2.0;
				the = pitch / 2.0;
				psi = yaw / 2.0;
			  
				btQuaternion orn(
								sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
								cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
								cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
								 cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
			  
				orn.normalize();
				tr.setRotation(orn);
			}
		}
	}
	return true;
}

void DynamicsMotionState::getWorldTransform(btTransform& transform) const
{
//  DALI_LOG_INFO(Debug::Filter::gDynamics, Debug::Verbose, "%s\n", __PRETTY_FUNCTION__);

  // get node's world position and rotation
  Vector3 position;
  Quaternion rotation;
  Vector3 axis;
  float angle( 0.0f );

  mDynamicsBody.GetNodePositionAndRotation(position, rotation);
  rotation.ToAxisAngle( axis, angle );

  // modify parameters
  transform.setIdentity();
  transform.setOrigin(btVector3(position.x, position.y, position.z));
  if( axis != Vector3::ZERO )
  {
    transform.setRotation( btQuaternion(btVector3(axis.x, axis.y, axis.z), btScalar(angle)) );
  }
}

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;
		mass = link->m_inertia.m_mass;
		inertialFrame = link->m_inertia.m_linkLocalFrame;
		localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
									  link->m_inertia.m_izz);
	}
	else
    {
        mass = 1.f;
        localInertiaDiagonal.setValue(1,1,1);
        inertialFrame.setIdentity();
    }
}

void LoadMeshFromCollada(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTransform, float& unitMeterScaling,int clientUpAxis)
{

	GLInstanceGraphicsShape* instance = 0;
	
	//usually COLLADA files don't have that many visual geometries/shapes
	visualShapes.reserve(32);

	float extraScaling = 1;//0.01;
	btHashMap<btHashString, int> name2ShapeIndex;
	b3FileUtils f;
	char filename[1024];
	if (!f.findFile(relativeFileName,filename,1024))
	{
		printf("File not found: %s\n", filename);
		return;
	}
	 
	TiXmlDocument doc(filename);
	if (!doc.LoadFile())
		return;

	//We need units to be in meter, so apply a scaling using the asset/units meter 
	unitMeterScaling=1;
	upAxisTransform.setIdentity();

	//Also we can optionally compensate all transforms using the asset/up_axis as well as unit meter scaling
	getUnitMeterScalingAndUpAxisTransform(doc, upAxisTransform, unitMeterScaling,clientUpAxis);
	
	btMatrix4x4 ident;
	ident.setIdentity();

	readLibraryGeometries(doc, visualShapes, name2ShapeIndex, extraScaling);
	
	readVisualSceneInstanceGeometries(doc, name2ShapeIndex, visualShapeInstances);

}

  bool world_transform(std::string frame_id, 
		       const planning_models::KinematicState &state,
		       btTransform &transform) {
    if (!frame_id.compare(state.getKinematicModel()->getRoot()->getParentFrameId())) {
      //identity transform
      transform.setIdentity();
      return true;
    }

    if (!frame_id.compare(state.getKinematicModel()->getRoot()->getChildFrameId())) {
      transform = state.getRootTransform();
      return true;
    }

    const planning_models::KinematicState::LinkState *link =
      state.getLinkState(frame_id);
    if (!link) {
      ROS_ERROR("Unable to find link %s in kinematic state", frame_id.c_str());
      return false;
    }
    
    transform = link->getGlobalLinkTransform();
    return true;
  }			   

bool BulletURDFImporter::getJointInfo(int urdfLinkIndex, btTransform& parent2joint, btVector3& jointAxisInJointSpace, int& jointType, btScalar& jointLowerLimit, btScalar& jointUpperLimit, btScalar& jointDamping, btScalar& jointFriction) const
{
    jointLowerLimit = 0.f;
    jointUpperLimit = 0.f;
	jointDamping = 0.f;
	jointFriction = 0.f;

	UrdfLink* const* linkPtr = m_data->m_urdfParser.getModel().m_links.getAtIndex(urdfLinkIndex);
	btAssert(linkPtr);
	if (linkPtr)
	{
		UrdfLink* link = *linkPtr;
		
		
		if (link->m_parentJoint)
		{
			UrdfJoint* pj = link->m_parentJoint;
			parent2joint = pj->m_parentLinkToJointTransform;
			jointType = pj->m_type;
			jointAxisInJointSpace = pj->m_localJointAxis;
			jointLowerLimit = pj->m_lowerLimit;
			jointUpperLimit = pj->m_upperLimit;
			jointDamping = pj->m_jointDamping;
			jointFriction = pj->m_jointFriction;

			return true;
		} else
		{
			parent2joint.setIdentity();
			return false;
		}
	}
	
	return false;
	
}

void btTransformFromIrrlichtMatrix(const irr::core::matrix4& irrmat, btTransform &transform)
{
    transform.setIdentity();

    transform.setFromOpenGLMatrix(irrmat.pointer());
}

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

//.........这里部分代码省略.........
		world->getSolverInfo().m_solverMode |= SOLVER_RANDMIZE_ORDER;

#ifdef	USER_DEFINED_FRICTION_MODEL
	//user defined friction model is not supported in 'cache friendly' solver yet, so switch to old solver

		world->getSolverInfo().m_solverMode = SOLVER_RANDMIZE_ORDER;

#endif //USER_DEFINED_FRICTION_MODEL

#ifdef DO_BENCHMARK_PYRAMIDS
		world->getSolverInfo().m_numIterations = 4;
#endif //DO_BENCHMARK_PYRAMIDS

		m_dynamicsWorld->getDispatchInfo().m_enableSPU = true;
		m_dynamicsWorld->setGravity(btVector3(0,-10,0));



#ifdef USER_DEFINED_FRICTION_MODEL
	{
		//m_solver->setContactSolverFunc(ContactSolverFunc func,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE);
		solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,DEFAULT_CONTACT_SOLVER_TYPE);
		solver->SetFrictionSolverFunc(myFrictionModel,DEFAULT_CONTACT_SOLVER_TYPE,USER_CONTACT_SOLVER_TYPE1);
		solver->SetFrictionSolverFunc(myFrictionModel,USER_CONTACT_SOLVER_TYPE1,USER_CONTACT_SOLVER_TYPE1);
		//m_physicsEnvironmentPtr->setNumIterations(2);
	}
#endif //USER_DEFINED_FRICTION_MODEL



	int i;

	btTransform tr;
	tr.setIdentity();


	for (i=0;i<gNumObjects;i++)
	{
		if (i>0)
		{
			shapeIndex[i] = 1;//sphere
		}
		else
			shapeIndex[i] = 0;
	}

	if (useCompound)
	{
		btCompoundShape* compoundShape = new btCompoundShape();
		btCollisionShape* oldShape = m_collisionShapes[1];
		m_collisionShapes[1] = compoundShape;
		btVector3 sphereOffset(0,0,2);

		comOffset.setIdentity();

#ifdef CENTER_OF_MASS_SHIFT
		comOffset.setOrigin(comOffsetVec);
		compoundShape->addChildShape(comOffset,oldShape);

#else
		compoundShape->addChildShape(tr,oldShape);
		tr.setOrigin(sphereOffset);
		compoundShape->addChildShape(tr,new btSphereShape(0.9));
#endif
	}

	EGLRendererObjectArray()
	{
		m_worldTransform.setIdentity();
		m_localScaling.setValue(1, 1, 1);
		m_graphicsInstanceId = -1;
	}