float SSVTreeCollider::LssLssDist(float r0, const Point& point0, const Point& point1, float r1, const Point& point2, const Point& point3)
{
Point p0, p1;
TransformPoint(p0, point0, mR0to1, mT0to1);
TransformPoint(p1, point1, mR0to1, mT0to1);
float d = SegSegDist(p0, p1, point2, point3);
return d - r0 - r1;
}
double CdShape::distance(const CdShape *i_shape) const
{
double d=0;
if (m_type == SPHERE && i_shape->type() == SPHERE){
d = (m_absP[0]-i_shape->center()).norm();
}else if (m_type == CAPSULE && i_shape->type() == SPHERE){
d = pointLineSegmentDistance(i_shape->center(), m_absP[0], m_absP[1]);
}else if (m_type == SPHERE && i_shape->type() == CAPSULE){
d = pointLineSegmentDistance(m_absP[0], i_shape->center(0), i_shape->center(1));
}else if (m_type == CAPSULE && i_shape->type() == CAPSULE){
d = SegSegDist(m_absP[0], m_absP[1], i_shape->center(0), i_shape->center(1));
}else{
std::cerr << "unknown type of pair:" << m_type << ","
<< i_shape->type() << std::endl;
}
return d - (m_radius + i_shape->radius());
}
