C++ Quaterniond::inverse方法代码示例

void doCapsuleSphereTest(double capsuleHeight, double capsuleRadius,
						 const Vector3d& capsulePosition, const Quaterniond& capsuleQuat,
						 double sphereRadius, const Vector3d& spherePosition, const Quaterniond& sphereQuat,
						 bool hasContacts, double depth,
						 const Vector3d& sphereProjection = Vector3d::Zero(),
						 const Vector3d& expectedNorm = Vector3d::Zero())
{
	std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(
		makeCapsuleRepresentation(capsuleHeight, capsuleRadius, capsuleQuat, capsulePosition),
		makeSphereRepresentation(sphereRadius, sphereQuat, spherePosition));

	CapsuleSphereDcdContact calc;
	calc.calculateContact(pair);
	EXPECT_EQ(hasContacts, pair->hasContacts());

	if (pair->hasContacts())
	{
		std::shared_ptr<Contact> contact(pair->getContacts().front());

		EXPECT_TRUE(eigenEqual(expectedNorm, contact->normal));
		EXPECT_NEAR(depth, contact->depth, SurgSim::Math::Geometry::DistanceEpsilon);
		EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
		EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());

		Vector3d capsuleLocalNormal = capsuleQuat.inverse() * expectedNorm;
		Vector3d penetrationPoint0(sphereProjection - capsuleLocalNormal * capsuleRadius);
		Vector3d sphereLocalNormal = sphereQuat.inverse() * expectedNorm;
		Vector3d penetrationPoint1(sphereLocalNormal * sphereRadius);
		EXPECT_TRUE(eigenEqual(penetrationPoint0, contact->penetrationPoints.first.rigidLocalPosition.getValue()));
		EXPECT_TRUE(eigenEqual(penetrationPoint1, contact->penetrationPoints.second.rigidLocalPosition.getValue()));
	}
}

void doSphereDoubleSidedPlaneTest(std::shared_ptr<SphereShape> sphere,
								  const Quaterniond& sphereQuat,
								  const Vector3d& sphereTrans,
								  std::shared_ptr<DoubleSidedPlaneShape> plane,
								  const Quaterniond& planeQuat,
								  const Vector3d& planeTrans,
								  bool expectedIntersect,
								  const double& expectedDepth = 0 ,
								  const Vector3d& expectedNorm = Vector3d::Zero())
{
	using SurgSim::Math::Geometry::ScalarEpsilon;
	using SurgSim::Math::Geometry::DistanceEpsilon;

	std::shared_ptr<ShapeCollisionRepresentation> planeRep =
		std::make_shared<ShapeCollisionRepresentation>("Collision Plane");
	planeRep->setShape(plane);
	planeRep->setLocalPose(SurgSim::Math::makeRigidTransform(planeQuat, planeTrans));

	std::shared_ptr<ShapeCollisionRepresentation> sphereRep =
		std::make_shared<ShapeCollisionRepresentation>("Collision Sphere");
	sphereRep->setShape(sphere);
	sphereRep->setLocalPose(SurgSim::Math::makeRigidTransform(sphereQuat, sphereTrans));

	SphereDoubleSidedPlaneContact calcNormal;
	std::shared_ptr<CollisionPair> pair = std::make_shared<CollisionPair>(sphereRep, planeRep);

	// Again this replicates the way this is calculated in the contact calculation just with different
	// starting values
	Vector3d sphereLocalNormal = sphereQuat.inverse() * expectedNorm;
	Vector3d spherePenetration = -sphereLocalNormal * sphere->getRadius();
	Vector3d planePenetration = -sphereLocalNormal * (sphere->getRadius() - expectedDepth);
	planePenetration = (sphereQuat * planePenetration) + sphereTrans;
	planePenetration = planeQuat.inverse() * (planePenetration - planeTrans);

	calcNormal.calculateContact(pair);
	if (expectedIntersect)
	{
		ASSERT_TRUE(pair->hasContacts());
		std::shared_ptr<Contact> contact = pair->getContacts().front();
		EXPECT_NEAR(expectedDepth, contact->depth, 1e-10);
		EXPECT_TRUE(eigenEqual(expectedNorm, contact->normal));
		EXPECT_TRUE(contact->penetrationPoints.first.rigidLocalPosition.hasValue());
		EXPECT_TRUE(contact->penetrationPoints.second.rigidLocalPosition.hasValue());
		EXPECT_TRUE(eigenEqual(spherePenetration,
							   contact->penetrationPoints.first.rigidLocalPosition.getValue()));
		EXPECT_TRUE(eigenEqual(planePenetration,
							   contact->penetrationPoints.second.rigidLocalPosition.getValue()));
	}
	else
	{
		EXPECT_FALSE(pair->hasContacts());
	}
}

inline Pose Pose::inverse()
{
	// Convention: x = R^T * (x' - T)
	//               = R^T * x' + (-R^T *T)
	Quaterniond inv_orientation = m_orientation.inverse();
	Vector3d inv_position = - (inv_orientation * m_position);

	return Pose(inv_orientation, inv_position);
}

inline Eigen::Vector3d
Pose::unmap(const Eigen::Vector3d& point) const
{
	// Convention: x = R^T * (x' - T)
	return m_orientation.inverse() * (point  -  m_position);
}

  // Set up spanning tree initialization
  void SysSPA::spanningTree(int node)
  {
    int nnodes = nodes.size();

    // set up an index from nodes to their constraints
    vector<vector<int> > cind;
    cind.resize(nnodes);

    for(size_t pi=0; pi<p2cons.size(); pi++)
      {
        ConP2 &con = p2cons[pi];
        int i0 = con.ndr;
        int i1 = con.nd1;
        cind[i0].push_back(i1);
        cind[i1].push_back(i0);        
      }

    // set up breadth-first algorithm
    VectorXd dist(nnodes);
    dist.setConstant(1e100);
    if (node >= nnodes)
      node = 0;
    dist[node] = 0.0;
    multimap<double,int> open;  // open list, priority queue - can have duplicates
    open.insert(std::make_pair<double,int>(0.0,int(node)));

    // do breadth-first computation
    while (!open.empty())
      {
        // get top node, remove it
        int ni = open.begin()->second;
        double di = open.begin()->first;
        open.erase(open.begin());
        if (di > dist[ni]) continue; // already dealt with

        // update neighbors
        Node &nd = nodes[ni];
        Matrix<double,3,4> n2w;
        transformF2W(n2w,nd.trans,nd.qrot); // from node to world coords

        vector<int> &nns = cind[ni];
        for (int i=0; i<(int)nns.size(); i++)
          {
            ConP2 &con = p2cons[nns[i]];
            double dd = con.tmean.norm(); // incremental distance
            // neighbor node index
            int nn = con.nd1;
            if (nn == ni)
              nn = con.ndr;
            Node &nd2 = nodes[nn];
            Vector3d tmean = con.tmean;
            Quaterniond qpmean = con.qpmean;
            if (nn == con.ndr)       // wrong way, reverse
              {
                qpmean = qpmean.inverse();
                tmean = nd.qrot.toRotationMatrix().transpose()*nd2.qrot.toRotationMatrix()*tmean;
              }
                
            if (dist[nn] > di + dd) // is neighbor now closer?
              {
                // set priority queue
                dist[nn] = di+dd;
                open.insert(std::make_pair<double,int>(di+dd,int(nn)));
                // update initial pose
                Vector4d trans;
                trans.head(3) = tmean;
                trans(3) = 1.0;
                nd2.trans.head(3) = n2w*trans;
                nd2.qrot = qpmean*nd.qrot;
                nd2.normRot();
                nd2.setTransform();
                nd2.setDr(true);
              }
          }
      }
    
  }