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