/* * SOLID - Software Library for Interference Detection * * Copyright (C) 2001-2003 Dtecta. All rights reserved. * * This library may be distributed under the terms of the Q Public License * (QPL) as defined by Trolltech AS of Norway and appearing in the file * LICENSE.QPL included in the packaging of this file. * * This library may be distributed and/or modified under the terms of the * GNU General Public License (GPL) version 2 as published by the Free Software * Foundation and appearing in the file LICENSE.GPL included in the * packaging of this file. * * This library is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. * * Commercial use or any other use of this library not covered by either * the QPL or the GPL requires an additional license from Dtecta. * Please contact info@dtecta.com for enquiries about the terms of commercial * use of this library. */ #include "DT_Object.h" #include "DT_AlgoTable.h" #include "DT_Convex.h" #include "DT_Complex.h" #include "DT_LineSegment.h" #include "DT_Transform.h" #include "DT_Minkowski.h" #include "DT_Sphere.h" void DT_Object::setBBox() { m_bbox = m_shape.bbox(m_xform, m_margin); DT_Vector3 min, max; m_bbox.getMin().getValue(min); m_bbox.getMax().getValue(max); T_ProxyList::const_iterator it; for (it = m_proxies.begin(); it != m_proxies.end(); ++it) { BP_SetBBox(*it, min, max); } } bool DT_Object::ray_cast(const MT_Point3& source, const MT_Point3& target, MT_Scalar& lambda, MT_Vector3& normal) const { MT_Transform inv_xform = m_xform.inverse(); MT_Point3 local_source = inv_xform(source); MT_Point3 local_target = inv_xform(target); MT_Vector3 local_normal; bool result = m_shape.ray_cast(local_source, local_target, lambda, local_normal); if (result) { normal = local_normal * inv_xform.getBasis(); MT_Scalar len = normal.length(); if (len > MT_Scalar(0.0)) { normal /= len; } } return result; } typedef AlgoTable IntersectTable; typedef AlgoTable Common_pointTable; typedef AlgoTable Penetration_depthTable; typedef AlgoTable Closest_pointsTable; bool intersectConvexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Vector3& v) { DT_Transform ta(a2w, (const DT_Convex&)a); DT_Transform tb(b2w, (const DT_Convex&)b); return intersect((a_margin > MT_Scalar(0.0) ? static_cast(DT_Minkowski(ta, DT_Sphere(a_margin))) : static_cast(ta)), (b_margin > MT_Scalar(0.0) ? static_cast(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast(tb)), v); } bool intersectComplexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Vector3& v) { if (a.getType() == COMPLEX) { DT_Transform tb(b2w, (const DT_Convex&)b); return intersect((const DT_Complex&)a, a2w, a_margin, (b_margin > MT_Scalar(0.0) ? static_cast(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast(tb)), v); } bool r = intersectComplexConvex(b, b2w, b_margin, a, a2w, a_margin, v); v *= -1.; return r; } bool intersectComplexComplex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Vector3& v) { return intersect((const DT_Complex&)a, a2w, a_margin, (const DT_Complex&)b, b2w, b_margin, v); } IntersectTable *intersectInitialize() { IntersectTable *p = new IntersectTable; p->addEntry(COMPLEX, COMPLEX, intersectComplexComplex); p->addEntry(COMPLEX, CONVEX, intersectComplexConvex); p->addEntry(CONVEX, CONVEX, intersectConvexConvex); return p; } bool intersect(const DT_Object& a, const DT_Object& b, MT_Vector3& v) { static IntersectTable *intersectTable = intersectInitialize(); Intersect intersect = intersectTable->lookup(a.getType(), b.getType()); return intersect(a.m_shape, a.m_xform, a.m_margin, b.m_shape, b.m_xform, b.m_margin, v); } bool common_pointConvexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) { DT_Transform ta(a2w, (const DT_Convex&)a); DT_Transform tb(b2w, (const DT_Convex&)b); return common_point((a_margin > MT_Scalar(0.0) ? static_cast(DT_Minkowski(ta, DT_Sphere(a_margin))) : static_cast(ta)), (b_margin > MT_Scalar(0.0) ? static_cast(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast(tb)), v, pa, pb); } bool common_pointComplexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) { if (a.getType() == COMPLEX) { DT_Transform tb(b2w, (const DT_Convex&)b); return common_point((const DT_Complex&)a, a2w, a_margin, (b_margin > MT_Scalar(0.0) ? static_cast(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast(tb)), v, pa, pb); } bool r = common_pointComplexConvex(b, b2w, b_margin, a, a2w, a_margin, v, pb, pa); v *= -1.; return r; } bool common_pointComplexComplex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) { return common_point((const DT_Complex&)a, a2w, a_margin, (const DT_Complex&)b, b2w, b_margin, v, pa, pb); } Common_pointTable *common_pointInitialize() { Common_pointTable *p = new Common_pointTable; p->addEntry(COMPLEX, COMPLEX, common_pointComplexComplex); p->addEntry(COMPLEX, CONVEX, common_pointComplexConvex); p->addEntry(CONVEX, CONVEX, common_pointConvexConvex); return p; } bool common_point(const DT_Object& a, const DT_Object& b, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) { static Common_pointTable *common_pointTable = common_pointInitialize(); Common_point common_point = common_pointTable->lookup(a.getType(), b.getType()); return common_point(a.m_shape, a.m_xform, a.m_margin, b.m_shape, b.m_xform, b.m_margin, v, pa, pb); } bool penetration_depthConvexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) { return hybrid_penetration_depth(DT_Transform(a2w, (const DT_Convex&)a), a_margin, DT_Transform(b2w, (const DT_Convex&)b), b_margin, v, pa, pb); } bool penetration_depthComplexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) { if (a.getType() == COMPLEX) return penetration_depth((const DT_Complex&)a, a2w, a_margin, DT_Transform(b2w, (const DT_Convex&)b), b_margin, v, pa, pb); bool r = penetration_depthComplexConvex(b, b2w, b_margin, a, a2w, a_margin, v, pb, pa); v *= -1.; return r; } bool penetration_depthComplexComplex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) { return penetration_depth((const DT_Complex&)a, a2w, a_margin, (const DT_Complex&)b, b2w, b_margin, v, pa, pb); } Penetration_depthTable *penetration_depthInitialize() { Penetration_depthTable *p = new Penetration_depthTable; p->addEntry(COMPLEX, COMPLEX, penetration_depthComplexComplex); p->addEntry(COMPLEX, CONVEX, penetration_depthComplexConvex); p->addEntry(CONVEX, CONVEX, penetration_depthConvexConvex); return p; } bool penetration_depth(const DT_Object& a, const DT_Object& b, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) { static Penetration_depthTable *penetration_depthTable = penetration_depthInitialize(); Penetration_depth penetration_depth = penetration_depthTable->lookup(a.getType(), b.getType()); return penetration_depth(a.m_shape, a.m_xform, a.m_margin, b.m_shape, b.m_xform, b.m_margin, v, pa, pb); } MT_Scalar closest_pointsConvexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Point3& pa, MT_Point3& pb) { DT_Transform ta(a2w, (const DT_Convex&)a); DT_Transform tb(b2w, (const DT_Convex&)b); return closest_points((a_margin > MT_Scalar(0.0) ? static_cast(DT_Minkowski(ta, DT_Sphere(a_margin))) : static_cast(ta)), (b_margin > MT_Scalar(0.0) ? static_cast(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast(tb)), MT_INFINITY, pa, pb); } MT_Scalar closest_pointsComplexConvex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Point3& pa, MT_Point3& pb) { if (a.getType() == COMPLEX) { DT_Transform tb(b2w, (const DT_Convex&)b); return closest_points((const DT_Complex&)a, a2w, a_margin, (b_margin > MT_Scalar(0.0) ? static_cast(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast(tb)), pa, pb); } return closest_pointsComplexConvex(b, b2w, b_margin, a, a2w, a_margin, pb, pa); } MT_Scalar closest_pointsComplexComplex(const DT_Shape& a, const MT_Transform& a2w, MT_Scalar a_margin, const DT_Shape& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Point3& pa, MT_Point3& pb) { return closest_points((const DT_Complex&)a, a2w, a_margin, (const DT_Complex&)b, b2w, b_margin, pa, pb); } Closest_pointsTable *closest_pointsInitialize() { Closest_pointsTable *p = new Closest_pointsTable; p->addEntry(COMPLEX, COMPLEX, closest_pointsComplexComplex); p->addEntry(COMPLEX, CONVEX, closest_pointsComplexConvex); p->addEntry(CONVEX, CONVEX, closest_pointsConvexConvex); return p; } MT_Scalar closest_points(const DT_Object& a, const DT_Object& b, MT_Point3& pa, MT_Point3& pb) { static Closest_pointsTable *closest_pointsTable = closest_pointsInitialize(); Closest_points closest_points = closest_pointsTable->lookup(a.getType(), b.getType()); return closest_points(a.m_shape, a.m_xform, a.m_margin, b.m_shape, b.m_xform, b.m_margin, pa, pb); }