C++ btVector3::setValue方法代码示例

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;
		if (link->m_parentJoint==0 && m_data->m_urdfParser.getModel().m_overrideFixedBase)
		{
			mass = 0.f;
			localInertiaDiagonal.setValue(0,0,0);
		}
		else
		{
			mass = link->m_inertia.m_mass;
			localInertiaDiagonal.setValue(link->m_inertia.m_ixx,link->m_inertia.m_iyy,
										  link->m_inertia.m_izz);
		}
		inertialFrame = link->m_inertia.m_linkLocalFrame;
		
	}
	else
    {
        mass = 1.f;
        localInertiaDiagonal.setValue(1,1,1);
        inertialFrame.setIdentity();
    }
}

static bool parseVector3(btVector3& vec3, const std::string& vector_str, ErrorLogger* logger, bool lastThree = false)
{
	vec3.setZero();
	btArray<std::string> pieces;
	btArray<float> rgba;
	urdfStringSplit(pieces, vector_str, urdfIsAnyOf(" "));
	for (int i = 0; i < pieces.size(); ++i)
	{
		if (!pieces[i].empty())
		{
			rgba.push_back(urdfLexicalCast<double>(pieces[i].c_str()));
		}
	}
	if (rgba.size() < 3)
	{
		logger->reportWarning("Couldn't parse vector3");
		return false;
	}
    if (lastThree) {
        vec3.setValue(rgba[rgba.size()-3], rgba[rgba.size()-2], rgba[rgba.size()-1]);
    }
    else
    {
        vec3.setValue(rgba[0],rgba[1],rgba[2]);

    }
	return true;
}

void	PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis)
{

	gEnableRealTimeSimVR = true;

	if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
	{
		printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS);
		return;
	}
	if (controllerId == gGraspingController)
	{
		gVRGripperAnalog = analogAxis;
		gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
		btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
		gVRGripperOrn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
	}
	else
	{
		gVRGripper2Analog = analogAxis;
		gVRController2Pos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
		btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]);
		gVRController2Orn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);
		
		m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
		m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]);
	}

}

void btSurfaceShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
	const felt::UrSurface3D::IsoGrid::Child& child =
		m_psurface->isogrid().children().get(m_pos_child);
	felt::Vec3i pos_min = child.offset();
	felt::Vec3i pos_max = pos_min + child.size().template cast<felt::INT>();

	aabbMin.setValue(btScalar(pos_min(0)),btScalar(pos_min(1)),btScalar(pos_min(2)));
	aabbMax.setValue(btScalar(pos_max(0)),btScalar(pos_max(1)),btScalar(pos_max(2)));
}

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

void btGImpactMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia) const
{

#ifdef CALC_EXACT_INERTIA
	inertia.setValue(0.f,0.f,0.f);

	int i = this->getMeshPartCount();
	btScalar partmass = mass/btScalar(i);

	while(i--)
	{
		btVector3 partinertia;
		getMeshPart(i)->calculateLocalInertia(partmass,partinertia);
		inertia+=partinertia;
	}

#else

	// Calc box inertia

	btScalar lx= m_localAABB.m_max[0] - m_localAABB.m_min[0];
	btScalar ly= m_localAABB.m_max[1] - m_localAABB.m_min[1];
	btScalar lz= m_localAABB.m_max[2] - m_localAABB.m_min[2];
	const btScalar x2 = lx*lx;
	const btScalar y2 = ly*ly;
	const btScalar z2 = lz*lz;
	const btScalar scaledmass = mass * btScalar(0.08333333);

	inertia = scaledmass * (btVector3(y2+z2,x2+z2,x2+y2));

#endif
}

//------------------------------------------------------------------------
//	NodeyBSPTree::calculateLocalInertia
//------------------------------------------------------------------------
//
void NodeyBSPTree::calculateLocalInertia( btScalar mass, btVector3& inertia ) const
{
	(void) mass;

	// Moving concave objects not supported.
	inertia.setValue( btScalar(0.),btScalar(0.),btScalar(0.) );
}

void	btTriangleMeshShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
{
	(void)mass;
	//moving concave objects not supported
	btAssert(0);
	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
}

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

void btStaticPlaneShape::getAabb(const btTransform& t,btVector3& aabbMin,btVector3& aabbMax) const
{
	(void)t;
	/*
	btVector3 infvec (btScalar(1e30),btScalar(1e30),btScalar(1e30));

	btVector3 center = m_planeNormal*m_planeConstant;
	aabbMin = center + infvec*m_planeNormal;
	aabbMax = aabbMin;
	aabbMin.setMin(center - infvec*m_planeNormal);
	aabbMax.setMax(center - infvec*m_planeNormal); 
	*/

	aabbMin.setValue(btScalar(-1e30),btScalar(-1e30),btScalar(-1e30));
	aabbMax.setValue(btScalar(1e30),btScalar(1e30),btScalar(1e30));

}

void btBU_Simplex1to4::getAabb(const btTransform& t, btVector3& aabbMin, btVector3& aabbMax) const
{
#if 1
	btPolyhedralConvexAabbCachingShape::getAabb(t, aabbMin, aabbMax);
#else
	aabbMin.setValue(BT_LARGE_FLOAT, BT_LARGE_FLOAT, BT_LARGE_FLOAT);
	aabbMax.setValue(-BT_LARGE_FLOAT,-BT_LARGE_FLOAT,-BT_LARGE_FLOAT);

	//just transform the vertices in worldspace, and take their AABB
	for (int i=0;i<m_numVertices;i++)
	{
		btVector3 worldVertex = t(m_vertices[i]);
		aabbMin.setMin(worldVertex);
		aabbMax.setMax(worldVertex);
	}
#endif
}

void	btStaticPlaneShape::calculateLocalInertia(btScalar mass,btVector3& inertia)
{
	(void)mass;

	//moving concave objects not supported
	
	inertia.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
}

bool parseVector3(const Value &val, btVector3 &retVec) {
	if (!val.isArray() && val.size() == 3) return false;
	if (!val[0].isConvertibleTo(ValueType::realValue)) return false;
	if (!val[1].isConvertibleTo(ValueType::realValue)) return false;
	if (!val[2].isConvertibleTo(ValueType::realValue)) return false;
	retVec.setValue(val[0].asDouble(), val[1].asDouble(),
					val[2].asDouble());
	return true;
}

void convertToBtTransform(const Vector3d& start_pos, const Vector3d& end_pos, btVector3& origin, btMatrix3x3& basis)
{
	Matrix3d rotation;
	rotation_from_tangent((start_pos - end_pos).normalized(), rotation);
	basis.setValue(rotation(0,0), rotation(0,1), rotation(0,2),
								 rotation(1,0), rotation(1,1), rotation(1,2),
								 rotation(2,0), rotation(2,1), rotation(2,2));
	Vector3d mid_point = (start_pos + end_pos)/2.0;
	origin.setValue(mid_point(0), mid_point(1), mid_point(2));
}

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