diff options
author | Michel Selten <michel@mselten.demon.nl> | 2003-12-06 22:02:42 +0300 |
---|---|---|
committer | Michel Selten <michel@mselten.demon.nl> | 2003-12-06 22:02:42 +0300 |
commit | 581c0f232cb083da326657681fdcd4abe1d14c7a (patch) | |
tree | 99009299ecc9e18af84de3d1d20f1dd6c93270c7 /extern/solid/src | |
parent | bef272a5d241ae3d73ccc124594efc53b484fec1 (diff) |
Added the Solid 3.5 sources to the blender source tree.
This is a direct copy from the CD-ROM contents except for the generated
libraries for the Windows platform. If needed, I can add those later on.
(Those take up over 800 kb).
All files, including license files, documentation, examples and sources are
committed.
Diffstat (limited to 'extern/solid/src')
59 files changed, 7528 insertions, 0 deletions
diff --git a/extern/solid/src/DT_AlgoTable.h b/extern/solid/src/DT_AlgoTable.h new file mode 100755 index 00000000000..0749ca7fdd9 --- /dev/null +++ b/extern/solid/src/DT_AlgoTable.h @@ -0,0 +1,47 @@ +/* + * 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. + */ + +#ifndef DT_ALGOTABLE_H +#define DT_ALGOTABLE_H + +#include "DT_Shape.h" + +template <typename Function, int NUM_TYPES = 8> +class AlgoTable { +public: + void addEntry(DT_ShapeType type1, DT_ShapeType type2, Function function) + { + table[type2][type1] = function; + table[type1][type2] = function; + } + + Function lookup(DT_ShapeType type1, DT_ShapeType type2) const + { + return table[type1][type2]; + } + +private: + Function table[NUM_TYPES][NUM_TYPES]; +}; + +#endif diff --git a/extern/solid/src/DT_C-api.cpp b/extern/solid/src/DT_C-api.cpp new file mode 100755 index 00000000000..c07a3bb7ccc --- /dev/null +++ b/extern/solid/src/DT_C-api.cpp @@ -0,0 +1,573 @@ +/* + * 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 "SOLID.h" + +#include "DT_Box.h" +#include "DT_Cone.h" +#include "DT_Cylinder.h" +#include "DT_Sphere.h" +#include "DT_Complex.h" +#include "DT_Polytope.h" +#include "DT_Polyhedron.h" +#include "DT_Point.h" +#include "DT_LineSegment.h" +#include "DT_Triangle.h" +#include "DT_Minkowski.h" +#include "DT_Hull.h" + +#include "DT_Response.h" +#include "DT_RespTable.h" + +#include "DT_Scene.h" +#include "DT_Object.h" + +#include "DT_VertexBase.h" + +#include "DT_Accuracy.h" + +typedef MT::Tuple3<DT_Scalar> T_Vertex; +typedef std::vector<T_Vertex> T_VertexBuf; +typedef std::vector<DT_Index> T_IndexBuf; +typedef std::vector<const DT_Convex *> T_PolyList; + +static T_VertexBuf vertexBuf; +static T_IndexBuf indexBuf; +static T_PolyList polyList; + +static DT_Complex *currentComplex = 0; +static DT_Polyhedron *currentPolyhedron = 0; +static DT_VertexBase *currentBase = 0; + + + + + + +DT_VertexBaseHandle DT_NewVertexBase(const void *pointer, DT_Size stride) +{ + return (DT_VertexBaseHandle)new DT_VertexBase(pointer, stride); +} + +void DT_DeleteVertexBase(DT_VertexBaseHandle vertexBase) +{ + delete (DT_VertexBase *)vertexBase; +} + +void DT_ChangeVertexBase(DT_VertexBaseHandle vertexBase, const void *pointer) +{ + DT_VertexBase *base = (DT_VertexBase *)vertexBase; + base->setPointer(pointer); + const DT_ComplexList& complexList = base->getComplexList(); + DT_ComplexList::const_iterator it; + for (it = complexList.begin(); it != complexList.end(); ++it) + { + (*it)->refit(); + } +} + + +DT_ShapeHandle DT_NewBox(DT_Scalar x, DT_Scalar y, DT_Scalar z) +{ + return (DT_ShapeHandle)new DT_Box(MT_Scalar(x) * MT_Scalar(0.5), + MT_Scalar(y) * MT_Scalar(0.5), + MT_Scalar(z) * MT_Scalar(0.5)); +} + +DT_ShapeHandle DT_NewCone(DT_Scalar radius, DT_Scalar height) +{ + return (DT_ShapeHandle)new DT_Cone(MT_Scalar(radius), MT_Scalar(height)); +} + +DT_ShapeHandle DT_NewCylinder(DT_Scalar radius, DT_Scalar height) +{ + return (DT_ShapeHandle)new DT_Cylinder(MT_Scalar(radius), MT_Scalar(height)); +} + +DT_ShapeHandle DT_NewSphere(DT_Scalar radius) +{ + return (DT_ShapeHandle)new DT_Sphere(MT_Scalar(radius)); +} + +DT_ShapeHandle DT_NewPoint(const DT_Vector3 point) +{ + return (DT_ShapeHandle)new DT_Point(MT_Point3(point)); +} + +DT_ShapeHandle DT_NewLineSegment(const DT_Vector3 source, const DT_Vector3 target) +{ + return (DT_ShapeHandle)new DT_LineSegment(MT_Point3(source), MT_Point3(target)); +} + +DT_ShapeHandle DT_NewMinkowski(DT_ShapeHandle shape1, DT_ShapeHandle shape2) +{ + if (((DT_Shape *)shape1)->getType() != CONVEX || + ((DT_Shape *)shape2)->getType() != CONVEX) + { + return 0; + } + + return (DT_ShapeHandle)new DT_Minkowski(*(DT_Convex *)shape1, *(DT_Convex *)shape2); +} + +DT_ShapeHandle DT_NewHull(DT_ShapeHandle shape1, DT_ShapeHandle shape2) +{ + if (((DT_Shape *)shape1)->getType() != CONVEX || + ((DT_Shape *)shape2)->getType() != CONVEX) + { + return 0; + } + + return (DT_ShapeHandle)new DT_Hull(*(DT_Convex *)shape1, *(DT_Convex *)shape2); +} + +DT_ShapeHandle DT_NewComplexShape(const DT_VertexBaseHandle vertexBase) +{ + if (!currentComplex) + { + currentBase = vertexBase ? (DT_VertexBase *)vertexBase : new DT_VertexBase; + currentComplex = new DT_Complex(currentBase); + } + return (DT_ShapeHandle)currentComplex; +} + +void DT_EndComplexShape() +{ + if (currentComplex) + { + if (currentBase->getPointer() == 0) + { + T_Vertex *vertexArray = new T_Vertex[vertexBuf.size()]; + assert(vertexArray); + std::copy(vertexBuf.begin(), vertexBuf.end(), &vertexArray[0]); + currentBase->setPointer(vertexArray, true); + } + + vertexBuf.clear(); + + currentComplex->finish(polyList.size(), &polyList[0]); + polyList.clear(); + currentComplex = 0; + currentBase = 0; + } +} + +DT_ShapeHandle DT_NewPolytope(const DT_VertexBaseHandle vertexBase) +{ + if (!currentPolyhedron) + { + currentBase = vertexBase ? (DT_VertexBase *)vertexBase : new DT_VertexBase; + currentPolyhedron = new DT_Polyhedron; + + } + return (DT_ShapeHandle)currentPolyhedron; +} + +void DT_EndPolytope() +{ + if (currentPolyhedron) + { + if (currentBase->getPointer() == 0) + { + currentBase->setPointer(&vertexBuf[0]); + new (currentPolyhedron) DT_Polyhedron(currentBase, indexBuf.size(), &indexBuf[0]); + + delete currentBase; + } + else + { + new (currentPolyhedron) DT_Polyhedron(currentBase, indexBuf.size(), &indexBuf[0]); + } + vertexBuf.clear(); + indexBuf.clear(); + currentPolyhedron = 0; + currentBase = 0; + } +} + +void DT_Begin() +{} + +void DT_End() +{ + if (currentComplex) + { + DT_VertexIndices(indexBuf.size(), &indexBuf[0]); + indexBuf.clear(); + } +} + +void DT_Vertex(const DT_Vector3 vertex) +{ + MT::Vector3<DT_Scalar> p(vertex); + int i = GEN_max((int)vertexBuf.size() - 20, 0); + int n = static_cast<int>(vertexBuf.size()); + + while (i != n && !(vertexBuf[i] == p)) + { + ++i; + } + + if (i == n) + { + vertexBuf.push_back(p); + } + indexBuf.push_back(i); +} + + +void DT_VertexIndex(DT_Index index) { indexBuf.push_back(index); } + +void DT_VertexIndices(DT_Count count, const DT_Index *indices) +{ + if (currentComplex) + { + DT_Convex *poly = count == 3 ? + static_cast<DT_Convex *>(new DT_Triangle(currentBase, indices[0], indices[1], indices[2])) : + static_cast<DT_Convex *>(new DT_Polytope(currentBase, count, indices)); + polyList.push_back(poly); + + } + + if (currentPolyhedron) + { + int i; + for (i = 0; i < count; ++i) + { + indexBuf.push_back(indices[i]); + } + } +} + +void DT_VertexRange(DT_Index first, DT_Count count) +{ + DT_Index *indices = new DT_Index[count]; + + DT_Index i; + for (i = 0; i != count; ++i) + { + indices[i] = first + i; + } + DT_VertexIndices(count, indices); + + delete [] indices; +} + +void DT_DeleteShape(DT_ShapeHandle shape) +{ + delete (DT_Shape *)shape; +} + + + + +// Scene + + +DT_SceneHandle DT_CreateScene() +{ + return (DT_SceneHandle)new DT_Scene; +} + +void DT_DestroyScene(DT_SceneHandle scene) +{ + delete (DT_Scene *)scene; +} + +void DT_AddObject(DT_SceneHandle scene, DT_ObjectHandle object) +{ + assert(scene); + assert(object); + ((DT_Scene *)scene)->addObject(*(DT_Object *)object); +} + +void DT_RemoveObject(DT_SceneHandle scene, DT_ObjectHandle object) +{ + assert(scene); + assert(object); + ((DT_Scene *)scene)->removeObject(*(DT_Object *)object); +} + + +// Object instantiation + + +DT_ObjectHandle DT_CreateObject(void *client_object, + DT_ShapeHandle shape) +{ + return (DT_ObjectHandle)new DT_Object(client_object, *(DT_Shape *)shape); +} + +void DT_DestroyObject(DT_ObjectHandle object) +{ + delete (DT_Object *)object; +} + +void DT_SetMargin(DT_ObjectHandle object, DT_Scalar margin) +{ + ((DT_Object *)object)->setMargin(MT_Scalar(margin)); +} + + +void DT_SetScaling(DT_ObjectHandle object, const DT_Vector3 scaling) +{ + ((DT_Object *)object)->setScaling(MT_Vector3(scaling)); +} + +void DT_SetPosition(DT_ObjectHandle object, const DT_Vector3 position) +{ + ((DT_Object *)object)->setPosition(MT_Point3(position)); +} + +void DT_SetOrientation(DT_ObjectHandle object, const DT_Quaternion orientation) +{ + ((DT_Object *)object)->setOrientation(MT_Quaternion(orientation)); +} + + +void DT_SetMatrixf(DT_ObjectHandle object, const float *m) +{ + ((DT_Object *)object)->setMatrix(m); +} + +void DT_GetMatrixf(DT_ObjectHandle object, float *m) +{ + ((DT_Object *)object)->getMatrix(m); +} + +void DT_SetMatrixd(DT_ObjectHandle object, const double *m) +{ + ((DT_Object *)object)->setMatrix(m); +} +void DT_GetMatrixd(DT_ObjectHandle object, double *m) +{ + ((DT_Object *)object)->getMatrix(m); +} + +void DT_GetBBox(DT_ObjectHandle object, DT_Vector3 min, DT_Vector3 max) +{ + const MT_BBox& bbox = ((DT_Object *)object)->getBBox(); + bbox.getMin().getValue(min); + bbox.getMax().getValue(max); +} + +DT_Scalar DT_GetClosestPair(DT_ObjectHandle object1, DT_ObjectHandle object2, + DT_Vector3 point1, DT_Vector3 point2) +{ + MT_Point3 p1, p2; + + MT_Scalar result = closest_points(*(DT_Object *)object1, + *(DT_Object *)object2, + p1, p2); + p1.getValue(point1); + p2.getValue(point2); + + return MT_sqrt(result); +} + +DT_Bool DT_GetCommonPoint(DT_ObjectHandle object1, DT_ObjectHandle object2, + DT_Vector3 point) +{ + MT_Point3 p1, p2; + MT_Vector3 v(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); + + bool result = common_point(*(DT_Object *)object1, *(DT_Object *)object2, v, p1, p2); + + if (result) + { + p1.getValue(point); + } + + return result; +} + +DT_Bool DT_GetPenDepth(DT_ObjectHandle object1, DT_ObjectHandle object2, + DT_Vector3 point1, DT_Vector3 point2) +{ + MT_Point3 p1, p2; + MT_Vector3 v(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); + + bool result = penetration_depth(*(DT_Object *)object1, *(DT_Object *)object2, v, p1, p2); + + if (result) + { + p1.getValue(point1); + p2.getValue(point2); + } + + return result; +} + +// Response + +DT_RespTableHandle DT_CreateRespTable() +{ + return (DT_RespTableHandle)new DT_RespTable; +} + +void DT_DestroyRespTable(DT_RespTableHandle respTable) +{ + delete (DT_RespTable *)respTable; +} + +DT_ResponseClass DT_GenResponseClass(DT_RespTableHandle respTable) +{ + return ((DT_RespTable *)respTable)->genResponseClass(); +} + +void DT_SetResponseClass(DT_RespTableHandle respTable, DT_ObjectHandle object, + DT_ResponseClass responseClass) +{ + ((DT_RespTable *)respTable)->setResponseClass(object, responseClass); +} + +void DT_ClearResponseClass(DT_RespTableHandle respTable, + DT_ObjectHandle object) +{ + ((DT_RespTable *)respTable)->clearResponseClass(object); +} + +void DT_CallResponse(DT_RespTableHandle respTable, + DT_ObjectHandle object1, + DT_ObjectHandle object2, + const DT_CollData *coll_data) +{ + const DT_ResponseList& responseList = + ((DT_RespTable *)respTable)->find(object1, object2); + + if (responseList.getType() != DT_NO_RESPONSE) + { + responseList(((DT_Object *)object1)->getClientObject(), + ((DT_Object *)object2)->getClientObject(), + coll_data); + } +} + + +void DT_AddDefaultResponse(DT_RespTableHandle respTable, + DT_ResponseCallback response, + DT_ResponseType type, void *client_data) +{ + ((DT_RespTable *)respTable)->addDefault(DT_Response(response, type, client_data)); +} + +void DT_RemoveDefaultResponse(DT_RespTableHandle respTable, + DT_ResponseCallback response) +{ + ((DT_RespTable *)respTable)->removeDefault(DT_Response(response)); +} + +void DT_AddClassResponse(DT_RespTableHandle respTable, + DT_ResponseClass responseClass, + DT_ResponseCallback response, + DT_ResponseType type, void *client_data) +{ + ((DT_RespTable *)respTable)->addSingle(responseClass, + DT_Response(response, type, client_data)); +} + +void DT_RemoveClassResponse(DT_RespTableHandle respTable, + DT_ResponseClass responseClass, + DT_ResponseCallback response) +{ + ((DT_RespTable *)respTable)->removeSingle(responseClass, + DT_Response(response)); +} + +void DT_AddPairResponse(DT_RespTableHandle respTable, + DT_ResponseClass responseClass1, + DT_ResponseClass responseClass2, + DT_ResponseCallback response, + DT_ResponseType type, void *client_data) +{ + ((DT_RespTable *)respTable)->addPair(responseClass1, responseClass2, + DT_Response(response, type, client_data)); +} + +void DT_RemovePairResponse(DT_RespTableHandle respTable, + DT_ResponseClass responseClass1, + DT_ResponseClass responseClass2, + DT_ResponseCallback response) +{ + ((DT_RespTable *)respTable)->removePair(responseClass1, responseClass2, + DT_Response(response)); +} + + +// Runtime + +void DT_SetAccuracy(DT_Scalar max_error) +{ + if (max_error > MT_Scalar(0.0)) + { + DT_Accuracy::setAccuracy(MT_Scalar(max_error)); + } +} + +void DT_SetTolerance(DT_Scalar tol_error) +{ + if (tol_error > MT_Scalar(0.0)) + { + DT_Accuracy::setTolerance(MT_Scalar(tol_error)); + } +} + +DT_Count DT_Test(DT_SceneHandle scene, DT_RespTableHandle respTable) +{ + return ((DT_Scene *)scene)->handleCollisions((DT_RespTable *)respTable); +} + +void *DT_RayCast(DT_SceneHandle scene, void *ignore_client, + const DT_Vector3 source, const DT_Vector3 target, + DT_Scalar max_param, DT_Scalar *param, DT_Vector3 normal) +{ + DT_Scalar lambda = max_param; + + void *client_object = ((DT_Scene *)scene)->rayCast(ignore_client, source, target, + lambda, normal); + if (client_object) + { + *param = lambda; + } + return client_object; +} + +DT_Bool DT_ObjectRayCast(DT_ObjectHandle object, + const DT_Vector3 source, const DT_Vector3 target, + DT_Scalar max_param, DT_Scalar *param, DT_Vector3 hit_normal) +{ + MT_Scalar lambda = MT_Scalar(max_param); + MT_Vector3 normal; + + bool result = ((DT_Object *)object)->ray_cast(MT_Point3(source), MT_Point3(target), + lambda, normal); + + if (result) + { + *param = lambda; + normal.getValue(hit_normal); + } + return result; +} + diff --git a/extern/solid/src/DT_Encounter.cpp b/extern/solid/src/DT_Encounter.cpp new file mode 100755 index 00000000000..49cc4bf3764 --- /dev/null +++ b/extern/solid/src/DT_Encounter.cpp @@ -0,0 +1,107 @@ +/* + * 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_RespTable.h" +#include "DT_Encounter.h" +#include "DT_Object.h" +#include "GEN_MinMax.h" + +DT_Bool DT_Encounter::exactTest(const DT_RespTable *respTable, int& count) const +{ + const DT_ResponseList& responseList = respTable->find(m_obj_ptr1, m_obj_ptr2); + + switch (responseList.getType()) + { + case DT_SIMPLE_RESPONSE: + if (intersect(*m_obj_ptr1, *m_obj_ptr2, m_sep_axis)) + { + ++count; + return (respTable->getResponseClass(m_obj_ptr1) < respTable->getResponseClass(m_obj_ptr2)) ? + responseList(m_obj_ptr1->getClientObject(), m_obj_ptr2->getClientObject(), 0) : + responseList(m_obj_ptr2->getClientObject(), m_obj_ptr1->getClientObject(), 0); + + } + break; + case DT_WITNESSED_RESPONSE: { + MT_Point3 p1, p2; + + if (common_point(*m_obj_ptr1, *m_obj_ptr2, m_sep_axis, p1, p2)) + { + ++count; + if (respTable->getResponseClass(m_obj_ptr1) < respTable->getResponseClass(m_obj_ptr2)) + { + DT_CollData coll_data; + + p1.getValue(coll_data.point1); + p2.getValue(coll_data.point2); + + return responseList(m_obj_ptr1->getClientObject(), m_obj_ptr2->getClientObject(), &coll_data); + } + else + { + DT_CollData coll_data; + + p1.getValue(coll_data.point2); + p2.getValue(coll_data.point1); + + return responseList(m_obj_ptr2->getClientObject(), m_obj_ptr1->getClientObject(), &coll_data); + } + } + break; + } + case DT_DEPTH_RESPONSE: { + MT_Point3 p1, p2; + + if (penetration_depth(*m_obj_ptr1, *m_obj_ptr2, m_sep_axis, p1, p2)) + { + ++count; + if (respTable->getResponseClass(m_obj_ptr1) < respTable->getResponseClass(m_obj_ptr2)) + { + DT_CollData coll_data; + + p1.getValue(coll_data.point1); + p2.getValue(coll_data.point2); + (p2 - p1).getValue(coll_data.normal); + + return responseList(m_obj_ptr1->getClientObject(), m_obj_ptr2->getClientObject(), &coll_data); + } + else + { + DT_CollData coll_data; + + p1.getValue(coll_data.point2); + p2.getValue(coll_data.point1); + (p1 - p2).getValue(coll_data.normal); + + return responseList(m_obj_ptr2->getClientObject(), m_obj_ptr1->getClientObject(), &coll_data); + } + } + break; + } + case DT_NO_RESPONSE: + break; + default: + assert(false); + } + return DT_CONTINUE; +} diff --git a/extern/solid/src/DT_Encounter.h b/extern/solid/src/DT_Encounter.h new file mode 100755 index 00000000000..f20ea3936b0 --- /dev/null +++ b/extern/solid/src/DT_Encounter.h @@ -0,0 +1,84 @@ +/* + * 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. + */ + +#ifndef DT_ENCOUNTER_H +#define DT_ENCOUNTER_H + +#include <set> + +#include "MT_Vector3.h" +#include "DT_Object.h" +#include "DT_Shape.h" + +class DT_RespTable; + +class DT_Encounter { +public: + DT_Encounter() {} + DT_Encounter(DT_Object *obj_ptr1, DT_Object *obj_ptr2) + : m_sep_axis(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)) + { + assert(obj_ptr1 != obj_ptr2); + if (obj_ptr2->getType() < obj_ptr1->getType() || + (obj_ptr2->getType() == obj_ptr1->getType() && + obj_ptr2 < obj_ptr1)) + { + m_obj_ptr1 = obj_ptr2; + m_obj_ptr2 = obj_ptr1; + } + else + { + m_obj_ptr1 = obj_ptr1; + m_obj_ptr2 = obj_ptr2; + } + } + + DT_Object *first() const { return m_obj_ptr1; } + DT_Object *second() const { return m_obj_ptr2; } + const MT_Vector3& separatingAxis() const { return m_sep_axis; } + + DT_Bool exactTest(const DT_RespTable *respTable, int& count) const; + +private: + DT_Object *m_obj_ptr1; + DT_Object *m_obj_ptr2; + mutable MT_Vector3 m_sep_axis; +}; + +inline bool operator<(const DT_Encounter& a, const DT_Encounter& b) +{ + return a.first() < b.first() || + (a.first() == b.first() && a.second() < b.second()); +} + + + +inline std::ostream& operator<<(std::ostream& os, const DT_Encounter& a) { + return os << '(' << a.first() << ", " << a.second() << ')'; +} + + + +typedef std::set<DT_Encounter> DT_EncounterTable; + +#endif diff --git a/extern/solid/src/DT_Object.cpp b/extern/solid/src/DT_Object.cpp new file mode 100755 index 00000000000..ba36c889b22 --- /dev/null +++ b/extern/solid/src/DT_Object.cpp @@ -0,0 +1,252 @@ +/* + * 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<Intersect> IntersectTable; +typedef AlgoTable<Common_point> Common_pointTable; +typedef AlgoTable<Penetration_depth> Penetration_depthTable; +typedef AlgoTable<Closest_points> 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<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : static_cast<const DT_Convex&>(ta)), + (b_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(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) +{ + 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<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(tb)), v); +} + +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<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : static_cast<const DT_Convex&>(ta)), + (b_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(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) +{ + 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<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(tb)), v, pa, pb); +} + +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) +{ + return penetration_depth((const DT_Complex&)a, a2w, a_margin, + DT_Transform(b2w, (const DT_Convex&)b), b_margin, v, pa, pb); +} + +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<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : static_cast<const DT_Convex&>(ta)), + (b_margin > MT_Scalar(0.0) ? static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(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) +{ + 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<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : static_cast<const DT_Convex&>(tb)), pa, pb); +} + +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); +} + diff --git a/extern/solid/src/DT_Object.h b/extern/solid/src/DT_Object.h new file mode 100755 index 00000000000..78beee2ab57 --- /dev/null +++ b/extern/solid/src/DT_Object.h @@ -0,0 +1,157 @@ +/* + * 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. + */ + +#ifndef DT_OBJECT_H +#define DT_OBJECT_H + +#include <vector> + +#include "SOLID.h" +#include "SOLID_broad.h" + +#include "MT_Transform.h" +#include "MT_Quaternion.h" +#include "MT_BBox.h" +#include "DT_Shape.h" + +class DT_Convex; + +class DT_Object { +public: + DT_Object(void *client_object, const DT_Shape& shape) : + m_client_object(client_object), + m_shape(shape), + m_margin(MT_Scalar(0.0)) + { + m_xform.setIdentity(); + setBBox(); + } + + void setMargin(MT_Scalar margin) + { + m_margin = margin; + setBBox(); + } + + void setScaling(const MT_Vector3& scaling) + { + m_xform.scale(scaling); + setBBox(); + } + + void setPosition(const MT_Point3& pos) + { + m_xform.setOrigin(pos); + setBBox(); + } + + void setOrientation(const MT_Quaternion& orn) + { + m_xform.setRotation(orn); + setBBox(); + } + + void setMatrix(const float *m) + { + m_xform.setValue(m); + assert(m_xform.getBasis().determinant() != MT_Scalar(0.0)); + setBBox(); + } + + void setMatrix(const double *m) + { + m_xform.setValue(m); + assert(m_xform.getBasis().determinant() != MT_Scalar(0.0)); + setBBox(); + } + + void getMatrix(float *m) const + { + m_xform.getValue(m); + } + + void getMatrix(double *m) const + { + m_xform.getValue(m); + } + + void setBBox(); + + const MT_BBox& getBBox() const { return m_bbox; } + + DT_ResponseClass getResponseClass() const { return m_responseClass; } + + void setResponseClass(DT_ResponseClass responseClass) + { + m_responseClass = responseClass; + } + + DT_ShapeType getType() const { return m_shape.getType(); } + + void *getClientObject() const { return m_client_object; } + + bool ray_cast(const MT_Point3& source, const MT_Point3& target, + MT_Scalar& param, MT_Vector3& normal) const; + + void addProxy(BP_ProxyHandle proxy) { m_proxies.push_back(proxy); } + + void removeProxy(BP_ProxyHandle proxy) + { + T_ProxyList::iterator it = std::find(m_proxies.begin(), m_proxies.end(), proxy); + if (it != m_proxies.end()) { + m_proxies.erase(it); + } + } + + + friend bool intersect(const DT_Object&, const DT_Object&, MT_Vector3& v); + + friend bool common_point(const DT_Object&, const DT_Object&, MT_Vector3&, + MT_Point3&, MT_Point3&); + + friend bool penetration_depth(const DT_Object&, const DT_Object&, + MT_Vector3&, MT_Point3&, MT_Point3&); + + friend MT_Scalar closest_points(const DT_Object&, const DT_Object&, + MT_Point3&, MT_Point3&); + +private: + typedef std::vector<BP_ProxyHandle> T_ProxyList; + + void *m_client_object; + DT_ResponseClass m_responseClass; + const DT_Shape& m_shape; + MT_Scalar m_margin; + MT_Transform m_xform; + T_ProxyList m_proxies; + MT_BBox m_bbox; +}; + +#endif + + + + + + + diff --git a/extern/solid/src/DT_RespTable.cpp b/extern/solid/src/DT_RespTable.cpp new file mode 100755 index 00000000000..20fbfc06aac --- /dev/null +++ b/extern/solid/src/DT_RespTable.cpp @@ -0,0 +1,184 @@ +/* + * 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_RespTable.h" + +#include <assert.h> + +DT_ResponseList DT_RespTable::g_emptyResponseList; + +DT_RespTable::~DT_RespTable() +{ + DT_ResponseClass i; + for (i = 0; i < m_responseClass; ++i) + { + delete [] m_table[i]; + } +} + +DT_ResponseClass DT_RespTable::genResponseClass() +{ + DT_ResponseClass newClass = m_responseClass++; + DT_ResponseList *newList = new DT_ResponseList[m_responseClass]; + assert(newList); + m_table.push_back(newList); + m_singleList.resize(m_responseClass); + DT_ResponseClass i; + for (i = 0; i < m_responseClass; ++i) + { + newList[i].append(m_default); + newList[i].append(m_singleList[i]); + } + return newClass; +} + +void DT_RespTable::setResponseClass(void *object, + DT_ResponseClass responseClass) +{ + assert(responseClass < m_responseClass); + m_objectMap[object] = responseClass; +} + +DT_ResponseClass DT_RespTable::getResponseClass(void *object) const +{ + T_ObjectMap::const_iterator it = m_objectMap.find(object); + assert(it != m_objectMap.end()); + return (*it).second; +} + +void DT_RespTable::clearResponseClass(void *object) +{ + m_objectMap.erase(object); +} + +const DT_ResponseList& DT_RespTable::find(void *object1, void *object2) const +{ + T_ObjectMap::const_iterator it = m_objectMap.find(object1); + if (it != m_objectMap.end()) + { + DT_ResponseClass responseClass1 = (*it).second; + it = m_objectMap.find(object2); + if (it != m_objectMap.end()) + { + DT_ResponseClass responseClass2 = (*it).second; + if (responseClass1 < responseClass2) + { + std::swap(responseClass1, responseClass2); + } + return m_table[responseClass1][responseClass2]; + } + } + return g_emptyResponseList; +} + +void DT_RespTable::addDefault(const DT_Response& response) +{ + m_default.addResponse(response); + DT_ResponseClass i; + for (i = 0; i < m_responseClass; ++i) + { + DT_ResponseClass j; + for (j = 0; j <= i; ++j) + { + m_table[i][j].addResponse(response); + } + } +} + +void DT_RespTable::removeDefault(const DT_Response& response) +{ + m_default.removeResponse(response); + DT_ResponseClass i; + for (i = 0; i < m_responseClass; ++i) + { + DT_ResponseClass j; + for (j = 0; j <= i; ++j) + { + m_table[i][j].removeResponse(response); + } + } +} + +void DT_RespTable::addSingle(DT_ResponseClass responseClass, + const DT_Response& response) +{ + assert(responseClass < m_responseClass); + m_singleList[responseClass].addResponse(response); + DT_ResponseClass j; + for (j = 0; j < responseClass; ++j) + { + m_table[responseClass][j].addResponse(response); + } + + DT_ResponseClass i; + for (i = responseClass; i < m_responseClass; ++i) + { + m_table[i][responseClass].addResponse(response); + } +} + +void DT_RespTable::removeSingle(DT_ResponseClass responseClass, + const DT_Response& response) +{ + assert(responseClass < m_responseClass); + m_singleList[responseClass].removeResponse(response); + DT_ResponseClass j; + for (j = 0; j < responseClass; ++j) + { + m_table[responseClass][j].removeResponse(response); + } + + DT_ResponseClass i; + for (i = responseClass; i < m_responseClass; ++i) + { + m_table[i][responseClass].removeResponse(response); + } +} + +void DT_RespTable::addPair(DT_ResponseClass responseClass1, + DT_ResponseClass responseClass2, + const DT_Response& response) +{ + assert(responseClass1 < m_responseClass); + assert(responseClass2 < m_responseClass); + if (responseClass1 < responseClass2) + { + std::swap(responseClass1, responseClass2); + } + m_table[responseClass1][responseClass2].addResponse(response); +} + + +void DT_RespTable::removePair(DT_ResponseClass responseClass1, + DT_ResponseClass responseClass2, + const DT_Response& response) +{ + assert(responseClass1 < m_responseClass); + assert(responseClass2 < m_responseClass); + if (responseClass1 < responseClass2) + { + std::swap(responseClass1, responseClass2); + } + m_table[responseClass1][responseClass2].removeResponse(response); +} + diff --git a/extern/solid/src/DT_RespTable.h b/extern/solid/src/DT_RespTable.h new file mode 100755 index 00000000000..9a17f562937 --- /dev/null +++ b/extern/solid/src/DT_RespTable.h @@ -0,0 +1,139 @@ +/* + * 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. + */ + +#ifndef DT_RESPTABLE_H +#define DT_RESPTABLE_H + +#include <algorithm> +#include <vector> +#include <list> +#include <map> +#include "GEN_MinMax.h" +#include "DT_Response.h" + +class DT_ResponseList : public std::list<DT_Response> { +public: + DT_ResponseList() : m_type(DT_NO_RESPONSE) {} + + DT_ResponseType getType() const { return m_type; } + + void addResponse(const DT_Response& response) + { + if (response.getType() != DT_NO_RESPONSE) + { + push_back(response); + GEN_set_max(m_type, response.getType()); + } + } + + void removeResponse(const DT_Response& response) + { + iterator it = std::find(begin(), end(), response); + if (it != end()) + { + erase(it); + m_type = DT_NO_RESPONSE; + for (it = begin(); it != end(); ++it) + { + GEN_set_max(m_type, (*it).getType()); + } + } + } + + void append(const DT_ResponseList& responseList) + { + if (responseList.getType() != DT_NO_RESPONSE) + { + const_iterator it; + for (it = responseList.begin(); it != responseList.end(); ++it) + { + addResponse(*it); + } + } + } + + DT_Bool operator()(void *a, void *b, const DT_CollData *coll_data) const + { + DT_Bool done = DT_CONTINUE; + const_iterator it; + for (it = begin(); !done && it != end(); ++it) + { + done = (*it)(a, b, coll_data); + } + return done; + } + +private: + DT_ResponseType m_type; +}; + +class DT_RespTable { +private: + typedef std::map<void *, DT_ResponseClass> T_ObjectMap; + typedef std::vector<DT_ResponseList *> T_PairTable; + typedef std::vector<DT_ResponseList> T_SingleList; + +public: + DT_RespTable() : m_responseClass(0) {} + + ~DT_RespTable(); + + DT_ResponseClass genResponseClass(); + + void setResponseClass(void *object, DT_ResponseClass responseClass); + DT_ResponseClass getResponseClass(void *object) const; + + void clearResponseClass(void *object); + + const DT_ResponseList& find(void *object1, void *object2) const; + + void addDefault(const DT_Response& response); + void removeDefault(const DT_Response& response); + + void addSingle(DT_ResponseClass responseClass, + const DT_Response& response); + void removeSingle(DT_ResponseClass responseClass, + const DT_Response& response); + + void addPair(DT_ResponseClass responseClass1, + DT_ResponseClass responseClass2, + const DT_Response& response); + void removePair(DT_ResponseClass responseClass1, + DT_ResponseClass responseClass2, + const DT_Response& response); + +private: + static DT_ResponseList g_emptyResponseList; + + T_ObjectMap m_objectMap; + DT_ResponseClass m_responseClass; + T_PairTable m_table; + T_SingleList m_singleList; + DT_ResponseList m_default; +}; + +#endif + + + + diff --git a/extern/solid/src/DT_Response.h b/extern/solid/src/DT_Response.h new file mode 100755 index 00000000000..e58d9bb9944 --- /dev/null +++ b/extern/solid/src/DT_Response.h @@ -0,0 +1,63 @@ +/* + * 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. + */ + +#ifndef DT_RESPONSE_H +#define DT_RESPONSE_H + +#include "SOLID.h" + +class DT_Response { +public: + DT_Response(DT_ResponseCallback response = 0, + DT_ResponseType type = DT_NO_RESPONSE, + void *client_data = 0) + : m_response(response), + m_type(type), + m_client_data(client_data) {} + + DT_ResponseType getType() const { return m_type; } + + DT_Bool operator()(void *a, void *b, const DT_CollData *coll_data) const + { + return (*m_response)(m_client_data, a, b, coll_data); + } + + friend bool operator==(const DT_Response& a, const DT_Response& b) + { + return a.m_response == b.m_response; + } + + friend bool operator!=(const DT_Response& a, const DT_Response& b) + { + return a.m_response != b.m_response; + } + +private: + DT_ResponseCallback m_response; + DT_ResponseType m_type; + void *m_client_data; +}; + +#endif + + diff --git a/extern/solid/src/DT_Scene.cpp b/extern/solid/src/DT_Scene.cpp new file mode 100755 index 00000000000..56cea1633ca --- /dev/null +++ b/extern/solid/src/DT_Scene.cpp @@ -0,0 +1,183 @@ +/* + * 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_Scene.h" +#include "DT_Object.h" +#include "DT_Convex.h" + +//#define DEBUG + +static void beginOverlap(void *client_data, void *object1, void *object2) +{ + DT_Encounter e((DT_Object *)object1, (DT_Object *)object2); + DT_EncounterTable *encounterTable = static_cast<DT_EncounterTable *>(client_data); + +#ifdef DEBUG + std::cout << "Begin: " << e << std::endl; +#endif + + encounterTable->insert(e); +} + + +static void endOverlap(void *client_data, void *object1, void *object2) +{ + DT_Encounter e((DT_Object *)object1, (DT_Object *)object2); + DT_EncounterTable *encounterTable = static_cast<DT_EncounterTable *>(client_data); + +#ifdef DEBUG + std::cout << "End: " << e << std::endl; +#endif + + assert(encounterTable->find(e) != encounterTable->end()); + encounterTable->erase(e); +} + +struct DT_RayCastData { + DT_RayCastData(const void *ignore) + : m_ignore(ignore) + {} + + const void *m_ignore; + MT_Vector3 m_normal; +}; + +static bool objectRayCast(void *client_data, + void *object, + const DT_Vector3 source, + const DT_Vector3 target, + DT_Scalar *lambda) +{ + DT_RayCastData *data = static_cast<DT_RayCastData *>(client_data); + if (((DT_Object *)object)->getClientObject() != data->m_ignore) + { + MT_Scalar param = MT_Scalar(*lambda); + + if (((DT_Object *)object)->ray_cast(MT_Point3(source), MT_Point3(target), + param, data->m_normal)) + { + *lambda = param; + return true; + } + } + return false; +} + +DT_Scene::DT_Scene() + : m_broadphase(BP_CreateScene(&m_encounterTable, &beginOverlap, &endOverlap)) +{} + +DT_Scene::~DT_Scene() +{ + BP_DestroyScene(m_broadphase); +} + +void DT_Scene::addObject(DT_Object &object) +{ + const MT_BBox& bbox = object.getBBox(); + DT_Vector3 min, max; + bbox.getMin().getValue(min); + bbox.getMax().getValue(max); + BP_ProxyHandle proxy = BP_CreateProxy(m_broadphase, &object, min, max); + +#ifdef DEBUG + DT_EncounterTable::iterator it; + std::cout << "Add " << &object << ':'; + for (it = m_encounterTable.begin(); it != m_encounterTable.end(); ++it) { + std::cout << ' ' << (*it); + } + std::cout << std::endl; +#endif + object.addProxy(proxy); + m_objectList.push_back(std::make_pair(&object, proxy)); +} + + + +void DT_Scene::removeObject(DT_Object& object) +{ + T_ObjectList::iterator it = m_objectList.begin(); + + while (it != m_objectList.end() && &object != (*it).first) + { + ++it; + } + + if (it != m_objectList.end()) + { + object.removeProxy((*it).second); + BP_DestroyProxy(m_broadphase, (*it).second); + m_objectList.erase(it); + +#ifdef DEBUG + std::cout << "Remove " << &object << ':'; + DT_EncounterTable::iterator it; + for (it = m_encounterTable.begin(); it != m_encounterTable.end(); ++it) + { + std::cout << ' ' << (*it); + assert((*it).first() != &object && + (*it).second() != &object); + } + std::cout << std::endl; +#endif + } +} + + + +int DT_Scene::handleCollisions(const DT_RespTable *respTable) +{ + int count = 0; + + assert(respTable); + + DT_EncounterTable::iterator it; + for (it = m_encounterTable.begin(); it != m_encounterTable.end(); ++it) + { + if ((*it).exactTest(respTable, count)) + { + break; + } + + } + return count; +} + +void *DT_Scene::rayCast(const void *ignore_client, + const DT_Vector3 source, const DT_Vector3 target, + DT_Scalar& lambda, DT_Vector3 normal) const +{ + DT_RayCastData data(ignore_client); + DT_Object *object = (DT_Object *)BP_RayCast(m_broadphase, + &objectRayCast, + &data, + source, target, + &lambda); + if (object) + { + data.m_normal.getValue(normal); + return object->getClientObject(); + } + + return 0; +} diff --git a/extern/solid/src/DT_Scene.h b/extern/solid/src/DT_Scene.h new file mode 100755 index 00000000000..9b061910312 --- /dev/null +++ b/extern/solid/src/DT_Scene.h @@ -0,0 +1,57 @@ +/* + * 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. + */ + +#ifndef DT_SCENE_H +#define DT_SCENE_H + +#include <vector> + +#include "SOLID_broad.h" +#include "DT_Encounter.h" + +class DT_Object; +class DT_RespTable; + +class DT_Scene { +public: + DT_Scene(); + ~DT_Scene(); + + void addObject(DT_Object& object); + void removeObject(DT_Object& object); + + int handleCollisions(const DT_RespTable *respTable); + + void *rayCast(const void *ignore_client, + const DT_Vector3 source, const DT_Vector3 target, + DT_Scalar& lambda, DT_Vector3 normal) const; + +private: + typedef std::vector<std::pair<DT_Object *, BP_ProxyHandle> > T_ObjectList; + + BP_SceneHandle m_broadphase; + T_ObjectList m_objectList; + DT_EncounterTable m_encounterTable; +}; + +#endif diff --git a/extern/solid/src/broad/BP_C-api.cpp b/extern/solid/src/broad/BP_C-api.cpp new file mode 100755 index 00000000000..43e1172927b --- /dev/null +++ b/extern/solid/src/broad/BP_C-api.cpp @@ -0,0 +1,77 @@ +/* + * 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 "SOLID_broad.h" + +#include "BP_Scene.h" +#include "BP_Proxy.h" + +BP_SceneHandle BP_CreateScene(void *client_data, + BP_Callback beginOverlap, + BP_Callback endOverlap) +{ + return (BP_SceneHandle)new BP_Scene(client_data, + beginOverlap, + endOverlap); +} + + +void BP_DestroyScene(BP_SceneHandle scene) +{ + delete (BP_Scene *)scene; +} + + +BP_ProxyHandle BP_CreateProxy(BP_SceneHandle scene, void *object, + const DT_Vector3 min, const DT_Vector3 max) +{ + return (BP_ProxyHandle) + ((BP_Scene *)scene)->createProxy(object, min, max); +} + + +void BP_DestroyProxy(BP_SceneHandle scene, BP_ProxyHandle proxy) +{ + ((BP_Scene *)scene)->destroyProxy((BP_Proxy *)proxy); +} + + + +void BP_SetBBox(BP_ProxyHandle proxy, const DT_Vector3 min, const DT_Vector3 max) +{ + ((BP_Proxy *)proxy)->setBBox(min, max); +} + +void *BP_RayCast(BP_SceneHandle scene, + BP_RayCastCallback objectRayCast, + void *client_data, + const DT_Vector3 source, + const DT_Vector3 target, + DT_Scalar *lambda) +{ + return ((BP_Scene *)scene)->rayCast(objectRayCast, + client_data, + source, target, + *lambda); +} + diff --git a/extern/solid/src/broad/BP_Endpoint.h b/extern/solid/src/broad/BP_Endpoint.h new file mode 100755 index 00000000000..8de6e67ce65 --- /dev/null +++ b/extern/solid/src/broad/BP_Endpoint.h @@ -0,0 +1,108 @@ +/* + * 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. + */ + +#ifndef BP_ENDPOINT_H +#define BP_ENDPOINT_H + +#include "SOLID_types.h" + +class BP_Proxy; + +class BP_Link { +public: + BP_Link() {} + explicit BP_Link(BP_Proxy *proxy) : + m_proxy(proxy) + {} + + DT_Index m_index; + DT_Count m_count; + BP_Proxy *m_proxy; +}; + +typedef unsigned int Uint32; + +class BP_Endpoint { +public: + enum { + MINIMUM = 0x00000000, + MAXIMUM = 0x80000000, + TYPEBIT = 0x00000001 + }; + + BP_Endpoint() {} + BP_Endpoint(DT_Scalar pos, Uint32 type, BP_Link *link) + : m_link(link) + { + setPos(pos, type); + } + + DT_Scalar getPos() const { return m_pos; } + DT_Index& getIndex() const { return m_link->m_index; } + DT_Count& getCount() const { return m_link->m_count; } + BP_Proxy *getProxy() const { return m_link->m_proxy; } + + DT_Index getEndIndex() const { return (m_link + 1)->m_index; } + + Uint32 getType() const + { + return (m_bits & TYPEBIT) ? (~m_bits & MAXIMUM) : (m_bits & MAXIMUM); + } + + void setPos(DT_Scalar pos, Uint32 type) + { + m_pos = pos; + if ((m_bits & MAXIMUM) == type) + { + m_bits &= ~TYPEBIT; + } + else + { + m_bits |= TYPEBIT; + } + } + +private: + union { + DT_Scalar m_pos; + Uint32 m_bits; + }; + BP_Link *m_link; +}; + +inline bool operator<(const BP_Endpoint& a, const BP_Endpoint& b) +{ + return a.getPos() < b.getPos(); +} + +#endif + + + + + + + + + + diff --git a/extern/solid/src/broad/BP_EndpointList.cpp b/extern/solid/src/broad/BP_EndpointList.cpp new file mode 100755 index 00000000000..aa094ffeb0a --- /dev/null +++ b/extern/solid/src/broad/BP_EndpointList.cpp @@ -0,0 +1,237 @@ +/* + * 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 <float.h> +#include <algorithm> + +#include "BP_EndpointList.h" +#include "BP_Scene.h" +#include "BP_Proxy.h" +#include "BP_ProxyList.h" + +DT_Index BP_EndpointList::stab(const BP_Endpoint& pos, BP_ProxyList& proxies) const +{ + DT_Index result = std::upper_bound(begin(), end(), pos) - begin(); + + if (result != 0) + { + DT_Index i = result - 1; + DT_Count count = (*this)[i].getCount(); + while (count) + { + const BP_Endpoint& endpoint = (*this)[i]; + if (endpoint.getType() == BP_Endpoint::MINIMUM && + pos < (*this)[endpoint.getEndIndex()]) + { + proxies.add(endpoint.getProxy()); + --count; + } + assert(i != 0 || count == 0); + --i; + } + } + return result; +} + +DT_Scalar BP_EndpointList::nextLambda(DT_Index& index, + DT_Scalar source, + DT_Scalar delta) const +{ + if (delta != 0.0f) + { + if (delta < 0.0f) + { + if (index != 0) + { + return ((*this)[--index].getPos() - source) / delta; + } + } + else + { + if (index != size()) + { + return ((*this)[index++].getPos() - source) / delta; + } + } + } + return FLT_MAX; +} + + +void BP_EndpointList::range(const BP_Endpoint& min, + const BP_Endpoint& max, + DT_Index& first, DT_Index& last, + BP_ProxyList& proxies) const +{ + first = stab(min, proxies); + last = std::upper_bound(begin(), end(), max) - begin(); + + DT_Index i; + for (i = first; i != last; ++i) + { + const BP_Endpoint& endpoint = (*this)[i]; + if (endpoint.getType() == BP_Endpoint::MINIMUM) + { + proxies.add(endpoint.getProxy()); + } + } +} + +void BP_EndpointList::addInterval(const BP_Endpoint& min, + const BP_Endpoint& max, + BP_ProxyList& proxies) +{ + assert(invariant()); + DT_Index first, last; + range(min, max, first, last, proxies); + insert(begin() + last, max); + insert(begin() + first, min); + ++last; + + (*this)[first].getCount() = first != 0 ? (*this)[first - 1].getCount() : 0; + (*this)[last].getCount() = (*this)[last - 1].getCount(); + + + DT_Index i; + for (i = first; i != last; ++i) + { + ++(*this)[i].getCount(); + (*this)[i].getIndex() = i; + } + for (; i != size(); ++i) + { + (*this)[i].getIndex() = i; + } + + assert(invariant()); +} + +void BP_EndpointList::removeInterval(DT_Index first, DT_Index last, + BP_ProxyList& proxies) +{ + assert(invariant()); + + BP_Endpoint min = (*this)[first]; + BP_Endpoint max = (*this)[last]; + + erase(begin() + last); + erase(begin() + first); + --last; + + DT_Index i; + for (i = first; i != last; ++i) + { + --(*this)[i].getCount(); + (*this)[i].getIndex() = i; + } + for (; i != size(); ++i) + { + (*this)[i].getIndex() = i; + } + + range(min, max, first, last, proxies); + + assert(invariant()); +} + +void BP_EndpointList::move(DT_Index index, DT_Scalar pos, Uint32 type, + BP_Scene& scene, T_Overlap overlap) +{ + assert(invariant()); + + BP_Endpoint endpoint = (*this)[index]; + DT_Scalar delta = pos - endpoint.getPos(); + + if (delta != DT_Scalar(0.0)) + { + endpoint.setPos(pos, type); + if (delta < DT_Scalar(0.0)) + { + while (index != 0 && endpoint < (*this)[index - 1]) + { + (*this)[index] = (*this)[index - 1]; + (*this)[index].getIndex() = index; + encounters((*this)[index], endpoint, scene, overlap); + --index; + } + } + else + { + DT_Index last = size() - 1; + while (index != last && (*this)[index + 1] < endpoint) + { + (*this)[index] = (*this)[index + 1]; + (*this)[index].getIndex() = index; + encounters(endpoint, (*this)[index], scene, overlap); + ++index; + } + } + (*this)[index] = endpoint; + (*this)[index].getIndex() = index; + } + + assert(invariant()); +} + +void BP_EndpointList::encounters(const BP_Endpoint& a, const BP_Endpoint& b, + BP_Scene& scene, T_Overlap overlap) +{ + assert(a.getProxy() != b.getProxy()); + + if (a.getType() != b.getType()) + { + if (a.getType() == BP_Endpoint::MAXIMUM) + { + if (overlap(*a.getProxy(), *b.getProxy())) + { + scene.callBeginOverlap(a.getProxy()->getObject(), + b.getProxy()->getObject()); + } + ++a.getCount(); + ++b.getCount(); + } + else + { + if (overlap(*a.getProxy(), *b.getProxy())) + { + scene.callEndOverlap(a.getProxy()->getObject(), + b.getProxy()->getObject()); + } + --a.getCount(); + --b.getCount(); + } + } + else + { + if (a.getType() == BP_Endpoint::MAXIMUM) + { + --a.getCount(); + ++b.getCount(); + } + else + { + ++a.getCount(); + --b.getCount(); + } + } +} diff --git a/extern/solid/src/broad/BP_EndpointList.h b/extern/solid/src/broad/BP_EndpointList.h new file mode 100755 index 00000000000..6154991ed3d --- /dev/null +++ b/extern/solid/src/broad/BP_EndpointList.h @@ -0,0 +1,109 @@ +/* + * 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. + */ + +#ifndef BP_ENDPOINTLIST_H +#define BP_ENDPOINTLIST_H + +//#define PARANOID + +#include <vector> + +#include "BP_Endpoint.h" +#include "BP_ProxyList.h" + +class BP_Scene; + +typedef bool (*T_Overlap)(const BP_Proxy& a, const BP_Proxy& b); + +class BP_EndpointList : public std::vector<BP_Endpoint> { +public: + BP_EndpointList() {} + + DT_Index stab(const BP_Endpoint& pos, BP_ProxyList& proxies) const; + + DT_Index stab(DT_Scalar pos, BP_ProxyList& proxies) const + { + return stab(BP_Endpoint(pos, BP_Endpoint::MINIMUM, 0), proxies); + } + + + void range(const BP_Endpoint& min, const BP_Endpoint& max, + DT_Index& first, DT_Index& last, BP_ProxyList& proxies) const; + + void range(DT_Scalar lb, DT_Scalar ub, DT_Index& first, DT_Index& last, BP_ProxyList& proxies) const + { + range(BP_Endpoint(lb, BP_Endpoint::MINIMUM, 0), + BP_Endpoint(ub, BP_Endpoint::MAXIMUM, 0), + first, last, proxies); + } + + void addInterval(const BP_Endpoint& min, const BP_Endpoint& max, BP_ProxyList& proxies); + void removeInterval(DT_Index first, DT_Index last, BP_ProxyList& proxies); + + void move(DT_Index index, DT_Scalar pos, Uint32 type, BP_Scene& scene, T_Overlap overlap); + + DT_Scalar nextLambda(DT_Index& index, DT_Scalar source, DT_Scalar target) const; + + +private: + void encounters(const BP_Endpoint& a, const BP_Endpoint& b, + BP_Scene& scene, T_Overlap overlap); + + +#ifdef PARANOID + bool invariant() const + { + DT_Count count = 0; + DT_Index i; + for (i = 0; i != size(); ++i) + { + const BP_Endpoint& endpoint = (*this)[i]; + + if (endpoint.getType() == BP_Endpoint::MINIMUM) + { + ++count; + } + else + { + --count; + } + if (endpoint.getCount() != count) + { + return false; + } + if (endpoint.getIndex() != i) + { + return false; + } + } + return true; + } +#else + bool invariant() const { return true; } +#endif + +}; + + + +#endif diff --git a/extern/solid/src/broad/BP_Proxy.cpp b/extern/solid/src/broad/BP_Proxy.cpp new file mode 100755 index 00000000000..e8007df240d --- /dev/null +++ b/extern/solid/src/broad/BP_Proxy.cpp @@ -0,0 +1,120 @@ +/* + * 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 <new> + +#include "BP_Proxy.h" +#include "BP_Scene.h" + +BP_Proxy::BP_Proxy(void *object, + BP_Scene& scene) + : m_object(object), + m_scene(scene) +{ + int i; + for (i = 0; i < 3; ++i) + { + new (&m_interval[i]) BP_Interval(this); + } +} + +void BP_Proxy::add(const DT_Vector3 min, + const DT_Vector3 max, + BP_ProxyList& proxies) +{ + int i; + for (i = 0; i < 3; ++i) + { + m_scene.getList(i).addInterval( + BP_Endpoint(min[i], BP_Endpoint::MINIMUM, &m_interval[i].m_min), + BP_Endpoint(max[i], BP_Endpoint::MAXIMUM, &m_interval[i].m_max), + proxies); + } +} + +void BP_Proxy::remove(BP_ProxyList& proxies) +{ + int i; + for (i = 0; i < 3; ++i) + { + m_scene.getList(i).removeInterval( + m_interval[i].m_min.m_index, + m_interval[i].m_max.m_index, + proxies); + } +} + +DT_Scalar BP_Proxy::getMin(int i) const +{ + return m_scene.getList(i)[m_interval[i].m_min.m_index].getPos(); +} + +DT_Scalar BP_Proxy::getMax(int i) const +{ + return m_scene.getList(i)[m_interval[i].m_max.m_index].getPos(); +} + +bool overlapXY(const BP_Proxy& a, const BP_Proxy& b) +{ + return a.getMin(0) <= b.getMax(0) && b.getMin(0) <= a.getMax(0) && + a.getMin(1) <= b.getMax(1) && b.getMin(1) <= a.getMax(1); +} + +bool overlapXZ(const BP_Proxy& a, const BP_Proxy& b) +{ + return a.getMin(0) <= b.getMax(0) && b.getMin(0) <= a.getMax(0) && + a.getMin(2) <= b.getMax(2) && b.getMin(2) <= a.getMax(2); +} + +bool overlapYZ(const BP_Proxy& a, const BP_Proxy& b) +{ + return a.getMin(1) <= b.getMax(1) && b.getMin(1) <= a.getMax(1) && + a.getMin(2) <= b.getMax(2) && b.getMin(2) <= a.getMax(2); +} + +void BP_Proxy::setBBox(const DT_Vector3 min, const DT_Vector3 max) +{ + static T_Overlap overlap[3] = { overlapYZ, overlapXZ, overlapXY }; + + int i; + for (i = 0; i < 3; ++i) + { + if (min[i] > getMax(i)) + { + m_scene.getList(i).move(m_interval[i].m_max.m_index, max[i], + BP_Endpoint::MAXIMUM, m_scene, overlap[i]); + m_scene.getList(i).move(m_interval[i].m_min.m_index, min[i], + BP_Endpoint::MINIMUM, m_scene, overlap[i]); + } + else + { + m_scene.getList(i).move(m_interval[i].m_min.m_index, min[i], + BP_Endpoint::MINIMUM, m_scene, overlap[i]); + m_scene.getList(i).move(m_interval[i].m_max.m_index, max[i], + BP_Endpoint::MAXIMUM, m_scene, overlap[i]); + } + } +} + + + diff --git a/extern/solid/src/broad/BP_Proxy.h b/extern/solid/src/broad/BP_Proxy.h new file mode 100755 index 00000000000..b4500ddca44 --- /dev/null +++ b/extern/solid/src/broad/BP_Proxy.h @@ -0,0 +1,78 @@ +/* + * 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. + */ + +#ifndef BP_PROXY_H +#define BP_PROXY_H + +#include "BP_Endpoint.h" +#include "BP_ProxyList.h" + +class BP_Interval { +public: + BP_Interval() {} + BP_Interval(BP_Proxy *proxy) : + m_min(proxy), + m_max(proxy) + {} + + BP_Link m_min; + BP_Link m_max; +}; + +class BP_Scene; + +class BP_Proxy { +public: + BP_Proxy(void *object, BP_Scene& scene); + + void add(const DT_Vector3 min, + const DT_Vector3 max, + BP_ProxyList& proxies); + + void remove(BP_ProxyList& proxies); + + void setBBox(const DT_Vector3 min, const DT_Vector3 max); + + void *getObject() { return m_object; } + + DT_Scalar getMin(int i) const; + DT_Scalar getMax(int i) const; + +private: + BP_Interval m_interval[3]; + void *m_object; + BP_Scene& m_scene; +}; + +inline bool BP_overlap(const BP_Proxy *a, const BP_Proxy *b) +{ + return a->getMin(0) <= b->getMax(0) && b->getMin(0) <= a->getMax(0) && + a->getMin(1) <= b->getMax(1) && b->getMin(1) <= a->getMax(1) && + a->getMin(2) <= b->getMax(2) && b->getMin(2) <= a->getMax(2); +} + +#endif + + + + diff --git a/extern/solid/src/broad/BP_ProxyList.h b/extern/solid/src/broad/BP_ProxyList.h new file mode 100755 index 00000000000..dde5693904a --- /dev/null +++ b/extern/solid/src/broad/BP_ProxyList.h @@ -0,0 +1,69 @@ +/* + * 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. + */ + +#ifndef BP_PROXYLIST_H +#define BP_PROXYLIST_H + +#include <assert.h> + +#include <vector> +#include <algorithm> +#include <utility> + +class BP_Proxy; + +typedef std::pair<BP_Proxy *, unsigned int> BP_ProxyEntry; + +inline bool operator<(const BP_ProxyEntry& a, const BP_ProxyEntry& b) +{ + return a.first < b.first; +} + +class BP_ProxyList : public std::vector<BP_ProxyEntry> { +public: + BP_ProxyList(size_t n = 20) : std::vector<BP_ProxyEntry>(n) {} + + iterator add(BP_Proxy *proxy) + { + BP_ProxyEntry entry = std::make_pair(proxy, 0); + iterator it = std::lower_bound(begin(), end(), entry); + if (it == end() || (*it).first != proxy) + { + it = insert(it, entry); + } + ++(*it).second; + return it; + } + + void remove(BP_Proxy *proxy) + { + BP_ProxyEntry entry = std::make_pair(proxy, 0); + iterator it = std::lower_bound(begin(), end(), entry); + if (it != end() && (*it).first == proxy && --(*it).second == 0) + { + erase(it); + } + } +}; + +#endif diff --git a/extern/solid/src/broad/BP_Scene.cpp b/extern/solid/src/broad/BP_Scene.cpp new file mode 100755 index 00000000000..c0cd83ba311 --- /dev/null +++ b/extern/solid/src/broad/BP_Scene.cpp @@ -0,0 +1,151 @@ +/* + * 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 "BP_Scene.h" +#include "BP_Proxy.h" + +#include <algorithm> + +BP_Proxy *BP_Scene::createProxy(void *object, + const DT_Vector3 min, + const DT_Vector3 max) +{ + BP_Proxy *proxy = new BP_Proxy(object, *this); + + proxy->add(min, max, m_proxies); + + BP_ProxyList::iterator it; + for (it = m_proxies.begin(); it != m_proxies.end(); ++it) + { + if ((*it).second == 3) + { + callBeginOverlap(proxy->getObject(), (*it).first->getObject()); + } + } + + m_proxies.clear(); + + return proxy; +} + +void BP_Scene::destroyProxy(BP_Proxy *proxy) +{ + proxy->remove(m_proxies); + + BP_ProxyList::iterator it; + for (it = m_proxies.begin(); it != m_proxies.end(); ++it) + { + if ((*it).second == 3) + { + callEndOverlap(proxy->getObject(), (*it).first->getObject()); + } + } + + m_proxies.clear(); + + delete proxy; +} + +void *BP_Scene::rayCast(BP_RayCastCallback objectRayCast, + void *client_data, + const DT_Vector3 source, + const DT_Vector3 target, + DT_Scalar& lambda) const +{ + void *client_object = 0; + + DT_Index index[3]; + index[0] = m_endpointList[0].stab(source[0], m_proxies); + index[1] = m_endpointList[1].stab(source[1], m_proxies); + index[2] = m_endpointList[2].stab(source[2], m_proxies); + + BP_ProxyList::iterator it; + for (it = m_proxies.begin(); it != m_proxies.end(); ++it) + { + if ((*it).second == 3 && + (*objectRayCast)(client_data, (*it).first->getObject(), source, target, &lambda)) + { + client_object = (*it).first->getObject(); + } + } + + DT_Vector3 delta; + delta[0] = target[0] - source[0]; + delta[1] = target[1] - source[1]; + delta[2] = target[2] - source[2]; + + DT_Vector3 lambdas; + lambdas[0] = m_endpointList[0].nextLambda(index[0], source[0], delta[0]); + lambdas[1] = m_endpointList[1].nextLambda(index[1], source[1], delta[1]); + lambdas[2] = m_endpointList[2].nextLambda(index[2], source[2], delta[2]); + int closest = lambdas[0] < lambdas[1] ? (lambdas[0] < lambdas[2] ? 0 : 2) : (lambdas[1] < lambdas[2] ? 1 : 2); + + while (lambdas[closest] < lambda) + { + if (delta[closest] < 0.0f) + { + const BP_Endpoint& endpoint = m_endpointList[closest][index[closest]]; + + if (endpoint.getType() == BP_Endpoint::MAXIMUM) + { + it = m_proxies.add(endpoint.getProxy()); + if ((*it).second == 3 && + (*objectRayCast)(client_data, (*it).first->getObject(), source, target, &lambda)) + { + client_object = (*it).first->getObject(); + } + } + else + { + m_proxies.remove(endpoint.getProxy()); + } + } + else + { + const BP_Endpoint& endpoint = m_endpointList[closest][index[closest] - 1]; + + if (endpoint.getType() == BP_Endpoint::MINIMUM) + { + it = m_proxies.add(endpoint.getProxy()); + if ((*it).second == 3 && + (*objectRayCast)(client_data, (*it).first->getObject(), source, target, &lambda)) + { + client_object = (*it).first->getObject(); + } + } + else + { + m_proxies.remove(endpoint.getProxy()); + } + } + + lambdas[closest] = m_endpointList[closest].nextLambda(index[closest], source[closest], delta[closest]); + closest = lambdas[0] < lambdas[1] ? (lambdas[0] < lambdas[2] ? 0 : 2) : (lambdas[1] < lambdas[2] ? 1 : 2); + } + + m_proxies.clear(); + + return client_object; +} + + diff --git a/extern/solid/src/broad/BP_Scene.h b/extern/solid/src/broad/BP_Scene.h new file mode 100755 index 00000000000..ef55374c43b --- /dev/null +++ b/extern/solid/src/broad/BP_Scene.h @@ -0,0 +1,79 @@ +/* + * 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. + */ + +#ifndef BP_SCENE_H +#define BP_SCENE_H + +#include <SOLID_broad.h> + +#include "BP_EndpointList.h" +#include "BP_ProxyList.h" + +class BP_Proxy; + +class BP_Scene { +public: + BP_Scene(void *client_data, + BP_Callback beginOverlap, + BP_Callback endOverlap) + : m_client_data(client_data), + m_beginOverlap(beginOverlap), + m_endOverlap(endOverlap), + m_proxies(20) + {} + + ~BP_Scene() {} + + BP_Proxy *createProxy(void *object, + const DT_Vector3 min, + const DT_Vector3 max); + + void destroyProxy(BP_Proxy *proxy); + + void *rayCast(BP_RayCastCallback objectRayCast, + void *client_data, + const DT_Vector3 source, + const DT_Vector3 target, + DT_Scalar& lambda) const; + + void callBeginOverlap(void *object1, void *object2) + { + (*m_beginOverlap)(m_client_data, object1, object2); + } + + void callEndOverlap(void *object1, void *object2) + { + (*m_endOverlap)(m_client_data, object1, object2); + } + + BP_EndpointList& getList(int i) { return m_endpointList[i]; } + +private: + void *m_client_data; + BP_Callback m_beginOverlap; + BP_Callback m_endOverlap; + BP_EndpointList m_endpointList[3]; + mutable BP_ProxyList m_proxies; +}; + +#endif diff --git a/extern/solid/src/complex/DT_BBoxTree.cpp b/extern/solid/src/complex/DT_BBoxTree.cpp new file mode 100755 index 00000000000..4f10f61f2e2 --- /dev/null +++ b/extern/solid/src/complex/DT_BBoxTree.cpp @@ -0,0 +1,90 @@ +/* + * 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_BBoxTree.h" + +inline DT_CBox getBBox(int first, int last, const DT_CBox *boxes, const DT_Index *indices) +{ + assert(last - first >= 1); + + DT_CBox bbox = boxes[indices[first]]; + int i; + for (i = first; i < last; ++i) + { + bbox = bbox.hull(boxes[indices[i]]); + } + + return bbox; +} + +DT_BBoxNode::DT_BBoxNode(int first, int last, int& node, DT_BBoxNode *free_nodes, const DT_CBox *boxes, DT_Index *indices, const DT_CBox& bbox) +{ + assert(last - first >= 2); + + int axis = bbox.longestAxis(); + MT_Scalar abscissa = bbox.getCenter()[axis]; + int i = first, mid = last; + while (i < mid) + { + if (boxes[indices[i]].getCenter()[axis] < abscissa) + { + ++i; + } + else + { + --mid; + std::swap(indices[i], indices[mid]); + } + } + + if (mid == first || mid == last) + { + mid = (first + last) / 2; + } + + m_lbox = getBBox(first, mid, boxes, indices); + m_rbox = getBBox(mid, last, boxes, indices); + m_flags = 0x0; + + if (mid - first == 1) + { + m_flags |= LLEAF; + m_lchild = indices[first]; + } + else + { + m_lchild = node++; + new(&free_nodes[m_lchild]) DT_BBoxNode(first, mid, node, free_nodes, boxes, indices, m_lbox); + } + + if (last - mid == 1) + { + m_flags |= RLEAF; + m_rchild = indices[mid]; + } + else + { + m_rchild = node++; + new(&free_nodes[m_rchild]) DT_BBoxNode(mid, last, node, free_nodes, boxes, indices, m_rbox); + } +} diff --git a/extern/solid/src/complex/DT_BBoxTree.h b/extern/solid/src/complex/DT_BBoxTree.h new file mode 100755 index 00000000000..995da05573a --- /dev/null +++ b/extern/solid/src/complex/DT_BBoxTree.h @@ -0,0 +1,540 @@ +/* + * 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. + */ + +#ifndef DT_BBOXTREE_H +#define DT_BBOXTREE_H + +#include <new> +#include <algorithm> + +#include "DT_Convex.h" +#include "DT_CBox.h" + + +class DT_BBoxTree { +public: + enum NodeType { INTERNAL = 0, LEAF = 1 }; + + DT_BBoxTree() {} + DT_BBoxTree(const DT_CBox& cbox, DT_Index index, NodeType type) + : m_cbox(cbox), + m_index(index), + m_type(type) + {} + + DT_CBox m_cbox; + DT_Index m_index; + NodeType m_type; +}; + + + +class DT_BBoxNode { +public: + DT_BBoxNode() {} + DT_BBoxNode(int first, int last, int& node, DT_BBoxNode *free_nodes, const DT_CBox *boxes, DT_Index *indices, const DT_CBox& bbox); + + void makeChildren(DT_BBoxTree& ltree, DT_BBoxTree& rtree) const; + void makeChildren(const DT_CBox& added, DT_BBoxTree& ltree, DT_BBoxTree& rtree) const; + + DT_CBox hull() const { return m_lbox.hull(m_rbox); } + + enum FlagType { LLEAF = 0x80, RLEAF = 0x40 }; + + DT_CBox m_lbox; + DT_CBox m_rbox; + DT_Index m_lchild; + DT_Index m_rchild; + unsigned char m_flags; +}; + +inline void DT_BBoxNode::makeChildren(DT_BBoxTree& ltree, DT_BBoxTree& rtree) const +{ + new (<ree) DT_BBoxTree(m_lbox, m_lchild, (m_flags & LLEAF) ? DT_BBoxTree::LEAF : DT_BBoxTree::INTERNAL); + new (&rtree) DT_BBoxTree(m_rbox, m_rchild, (m_flags & RLEAF) ? DT_BBoxTree::LEAF : DT_BBoxTree::INTERNAL); + +} + +inline void DT_BBoxNode::makeChildren(const DT_CBox& added, DT_BBoxTree& ltree, DT_BBoxTree& rtree) const +{ + new (<ree) DT_BBoxTree(m_lbox + added, m_lchild, (m_flags & LLEAF) ? DT_BBoxTree::LEAF : DT_BBoxTree::INTERNAL); + new (&rtree) DT_BBoxTree(m_rbox + added, m_rchild, (m_flags & RLEAF) ? DT_BBoxTree::LEAF : DT_BBoxTree::INTERNAL); +} + + +template <typename Shape> +class DT_RootData { +public: + DT_RootData(const DT_BBoxNode *nodes, + const Shape *leaves) + : m_nodes(nodes), + m_leaves(leaves) + {} + + const DT_BBoxNode *m_nodes; + const Shape *m_leaves; +}; + +template <typename Shape1, typename Shape2> +class DT_ObjectData : public DT_RootData<Shape1> { +public: + DT_ObjectData(const DT_BBoxNode *nodes, + const Shape1 *leaves, + const MT_Transform& xform, + Shape2 plus) + : DT_RootData<Shape1>(nodes, leaves), + m_xform(xform), + m_inv_xform(xform.inverse()), + m_plus(plus), + m_added(computeCBox(plus, m_inv_xform)) + {} + + const MT_Transform& m_xform; + MT_Transform m_inv_xform; + Shape2 m_plus; + DT_CBox m_added; +}; + +template <typename Shape1, typename Shape2> +class DT_Pack { +public: + DT_Pack(const DT_ObjectData<Shape1, Shape2>& a, const DT_Convex& b) + : m_a(a), + m_b(b), + m_b_cbox(b.bbox(m_a.m_inv_xform)) + {} + + DT_ObjectData<Shape1, Shape2> m_a; + const DT_Convex& m_b; + DT_CBox m_b_cbox; +}; + +template <typename Shape1, typename Shape2> +class DT_HybridPack : public DT_Pack<Shape1, Shape2> { +public: + DT_HybridPack(const DT_ObjectData<Shape1, Shape2>& a, const DT_Convex& b, MT_Scalar margin) + : DT_Pack<Shape1, Shape2>(a, b), + m_margin(margin) + { + m_b_cbox += computeCBox(margin, m_a.m_inv_xform); + } + + MT_Scalar m_margin; +}; + +template <typename Shape1, typename Shape2> +class DT_DuoPack { +public: + DT_DuoPack(const DT_ObjectData<Shape1, Shape2>& a, const DT_ObjectData<Shape1, Shape2>& b) + : m_a(a), + m_b(b) + { + m_b2a = a.m_inv_xform * b.m_xform; + m_a2b = b.m_inv_xform * a.m_xform; + m_abs_b2a = m_b2a.getBasis().absolute(); + m_abs_a2b = m_a2b.getBasis().absolute(); + } + + DT_ObjectData<Shape1, Shape2> m_a, m_b; + MT_Transform m_b2a, m_a2b; + MT_Matrix3x3 m_abs_b2a, m_abs_a2b; +}; + + +template <typename Shape> +inline void refit(DT_BBoxNode& node, const DT_RootData<Shape>& rd) +{ + node.m_lbox = (node.m_flags & DT_BBoxNode::LLEAF) ? + computeCBox(rd.m_leaves[node.m_lchild]) : + rd.m_nodes[node.m_lchild].hull(); + node.m_rbox = (node.m_flags & DT_BBoxNode::RLEAF) ? + computeCBox(rd.m_leaves[node.m_rchild]) : + rd.m_nodes[node.m_rchild].hull(); +} + + +template <typename Shape> +bool ray_cast(const DT_BBoxTree& a, const DT_RootData<Shape>& rd, + const MT_Point3& source, const MT_Point3& target, + MT_Scalar& lambda, MT_Vector3& normal) +{ + if (!a.m_cbox.overlapsLineSegment(source, source.lerp(target, lambda))) + { + return false; + } + + if (a.m_type == DT_BBoxTree::LEAF) + { + return ::ray_cast(rd, a.m_index, source, target, lambda, normal); + } + else + { + DT_BBoxTree ltree, rtree; + rd.m_nodes[a.m_index].makeChildren(ltree, rtree); + + bool lresult = ray_cast(ltree, rd, source, target, lambda, normal); + bool rresult = ray_cast(rtree, rd, source, target, lambda, normal); + return lresult || rresult; + } +} + + +#ifdef STATISTICS +int num_box_tests = 0; +#endif + +template <typename Shape1, typename Shape2> +inline bool intersect(const DT_CBox& a, const DT_CBox& b, const DT_DuoPack<Shape1, Shape2>& pack) +{ +#ifdef STATISTICS + ++num_box_tests; +#endif + + + MT_Vector3 abs_pos_b2a = (pack.m_b2a(b.getCenter()) - a.getCenter()).absolute(); + MT_Vector3 abs_pos_a2b = (pack.m_a2b(a.getCenter()) - b.getCenter()).absolute(); + return (a.getExtent()[0] + pack.m_abs_b2a[0].dot(b.getExtent()) >= abs_pos_b2a[0]) && + (a.getExtent()[1] + pack.m_abs_b2a[1].dot(b.getExtent()) >= abs_pos_b2a[1]) && + (a.getExtent()[2] + pack.m_abs_b2a[2].dot(b.getExtent()) >= abs_pos_b2a[2]) && + (b.getExtent()[0] + pack.m_abs_a2b[0].dot(a.getExtent()) >= abs_pos_a2b[0]) && + (b.getExtent()[1] + pack.m_abs_a2b[1].dot(a.getExtent()) >= abs_pos_a2b[1]) && + (b.getExtent()[2] + pack.m_abs_a2b[2].dot(a.getExtent()) >= abs_pos_a2b[2]); +} + + + + +template <typename Shape1, typename Shape2> +bool intersect(const DT_BBoxTree& a, const DT_Pack<Shape1, Shape2>& pack, MT_Vector3& v) +{ + if (!a.m_cbox.overlaps(pack.m_b_cbox)) + { + return false; + } + + if (a.m_type == DT_BBoxTree::LEAF) + { + return intersect(pack, a.m_index, v); + } + else + { + DT_BBoxTree a_ltree, a_rtree; + pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); + return intersect(a_ltree, pack, v) || intersect(a_rtree, pack, v); + } +} + +template <typename Shape1, typename Shape2> +bool intersect(const DT_BBoxTree& a, const DT_BBoxTree& b, const DT_DuoPack<Shape1, Shape2>& pack, MT_Vector3& v) +{ + if (!intersect(a.m_cbox, b.m_cbox, pack)) + { + return false; + } + + if (a.m_type == DT_BBoxTree::LEAF && b.m_type == DT_BBoxTree::LEAF) + { + return intersect(pack, a.m_index, b.m_index, v); + } + else if (a.m_type == DT_BBoxTree::LEAF || + (b.m_type != DT_BBoxTree::LEAF && a.m_cbox.size() < b.m_cbox.size())) + { + DT_BBoxTree b_ltree, b_rtree; + pack.m_b.m_nodes[b.m_index].makeChildren(pack.m_b.m_added, b_ltree, b_rtree); + + return intersect(a, b_ltree, pack, v) || intersect(a, b_rtree, pack, v); + } + else + { + DT_BBoxTree a_ltree, a_rtree; + pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); + return intersect(a_ltree, b, pack, v) || intersect(a_rtree, b, pack, v); + } +} + +template <typename Shape1, typename Shape2> +bool common_point(const DT_BBoxTree& a, const DT_Pack<Shape1, Shape2>& pack, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + if (!a.m_cbox.overlaps(pack.m_b_cbox)) + { + return false; + } + + if (a.m_type == DT_BBoxTree::LEAF) + { + return common_point(pack, a.m_index, v, pa, pb); + } + else + { + DT_BBoxTree a_ltree, a_rtree; + pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); + return common_point(a_ltree, pack, v, pa, pb) || + common_point(a_rtree, pack, v, pa ,pb); + } +} + +template <typename Shape1, typename Shape2> +bool common_point(const DT_BBoxTree& a, const DT_BBoxTree& b, const DT_DuoPack<Shape1, Shape2>& pack, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + if (!intersect(a.m_cbox, b.m_cbox, pack)) + { + return false; + } + + if (a.m_type == DT_BBoxTree::LEAF && b.m_type == DT_BBoxTree::LEAF) + { + return common_point(pack, a.m_index, b.m_index, v, pa, pb); + } + else if (a.m_type == DT_BBoxTree::LEAF || + (b.m_type != DT_BBoxTree::LEAF && a.m_cbox.size() < b.m_cbox.size())) + { + DT_BBoxTree b_ltree, b_rtree; + pack.m_b.m_nodes[b.m_index].makeChildren(pack.m_b.m_added, b_ltree, b_rtree); + return common_point(a, b_ltree, pack, v, pa, pb) || + common_point(a, b_rtree, pack, v, pa, pb); + } + else + { + DT_BBoxTree a_ltree, a_rtree; + pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); + return common_point(a_ltree, b, pack, v, pa, pb) || + common_point(a_rtree, b, pack, v, pa ,pb); + } +} + + +template <typename Shape1, typename Shape2> +bool penetration_depth(const DT_BBoxTree& a, const DT_HybridPack<Shape1, Shape2>& pack, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb, MT_Scalar& max_pen_len) +{ + if (!a.m_cbox.overlaps(pack.m_b_cbox)) + { + return false; + } + + if (a.m_type == DT_BBoxTree::LEAF) + { + if (penetration_depth(pack, a.m_index, v, pa, pb)) + { + max_pen_len = pa.distance2(pb); + return true; + } + else + { + return false; + } + } + else + { + DT_BBoxTree a_ltree, a_rtree; + pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); + if (penetration_depth(a_ltree, pack, v, pa, pb, max_pen_len)) + { + MT_Vector3 rv; + MT_Point3 rpa, rpb; + MT_Scalar rmax_pen_len; + if (penetration_depth(a_rtree, pack, rv, rpa, rpb, rmax_pen_len) && + (max_pen_len < rmax_pen_len)) + { + max_pen_len = rmax_pen_len; + v = rv; + pa = rpa; + pb = rpb; + } + return true; + } + else + { + return penetration_depth(a_rtree, pack, v, pa, pb, max_pen_len); + } + } +} + +template <typename Shape1, typename Shape2> +bool penetration_depth(const DT_BBoxTree& a, const DT_BBoxTree& b, const DT_DuoPack<Shape1, Shape2>& pack, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb, MT_Scalar& max_pen_len) +{ + if (!intersect(a.m_cbox, b.m_cbox, pack)) + { + return false; + } + + if (a.m_type == DT_BBoxTree::LEAF && b.m_type == DT_BBoxTree::LEAF) + { + if (penetration_depth(pack, a.m_index, b.m_index, v, pa, pb)) + { + max_pen_len = pa.distance2(pb); + return true; + } + else + { + return false; + } + } + else if (a.m_type == DT_BBoxTree::LEAF || + (b.m_type != DT_BBoxTree::LEAF && a.m_cbox.size() < b.m_cbox.size())) + { + DT_BBoxTree b_ltree, b_rtree; + pack.m_b.m_nodes[b.m_index].makeChildren(pack.m_b.m_added, b_ltree, b_rtree); + if (penetration_depth(a, b_ltree, pack, v, pa, pb, max_pen_len)) + { + MT_Point3 rpa, rpb; + MT_Scalar rmax_pen_len; + if (penetration_depth(a, b_rtree, pack, v, rpa, rpb, rmax_pen_len) && + (max_pen_len < rmax_pen_len)) + { + max_pen_len = rmax_pen_len; + pa = rpa; + pb = rpb; + } + return true; + } + else + { + return penetration_depth(a, b_rtree, pack, v, pa, pb, max_pen_len); + } + } + else + { + DT_BBoxTree a_ltree, a_rtree; + pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); + if (penetration_depth(a_ltree, b, pack, v, pa, pb, max_pen_len)) + { + MT_Point3 rpa, rpb; + MT_Scalar rmax_pen_len; + if (penetration_depth(a_rtree, b, pack, v, rpa, rpb, rmax_pen_len) && + (max_pen_len < rmax_pen_len)) + { + max_pen_len = rmax_pen_len; + pa = rpa; + pb = rpb; + } + return true; + } + else + { + return penetration_depth(a_rtree, b, pack, v, pa, pb, max_pen_len); + } + } +} + + +// Returns a lower bound for the distance for quick rejection in closest_points +inline MT_Scalar distance2(const DT_CBox& a, const MT_Transform& a2w, + const DT_CBox& b, const MT_Transform& b2w) +{ + MT_Vector3 v = b2w(b.getCenter()) - a2w(a.getCenter()); + MT_Scalar dist2 = v.length2(); + if (dist2 > MT_Scalar(0.0)) + { + MT_Vector3 w = b2w(b.support(-v * b2w.getBasis())) - a2w(a.support(v * a2w.getBasis())); + MT_Scalar delta = v.dot(w); + return delta > MT_Scalar(0.0) ? delta * delta / dist2 : MT_Scalar(0.0); + } + return MT_Scalar(0.0); +} + + +template <typename Shape1, typename Shape2> +MT_Scalar closest_points(const DT_BBoxTree& a, const DT_Pack<Shape1, Shape2>& pack, + MT_Scalar max_dist2, MT_Point3& pa, MT_Point3& pb) +{ + if (a.m_type == DT_BBoxTree::LEAF) + { + return closest_points(pack, a.m_index, max_dist2, pa, pb); + } + else + { + DT_BBoxTree a_ltree, a_rtree; + pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); + MT_Scalar ldist2 = distance2(a_ltree.m_cbox, pack.m_a.m_xform, pack.m_b_cbox, pack.m_a.m_xform); + MT_Scalar rdist2 = distance2(a_rtree.m_cbox, pack.m_a.m_xform, pack.m_b_cbox, pack.m_a.m_xform); + if (ldist2 < rdist2) + { + MT_Scalar dist2 = ldist2 < max_dist2 ? closest_points(a_ltree, pack, max_dist2, pa, pb) : MT_INFINITY; + GEN_set_min(max_dist2, dist2); + return rdist2 < max_dist2 ? GEN_min(dist2, closest_points(a_rtree, pack, max_dist2, pa, pb)) : dist2; + } + else + { + MT_Scalar dist2 = rdist2 < max_dist2 ? closest_points(a_rtree, pack, max_dist2, pa, pb) : MT_INFINITY; + GEN_set_min(max_dist2, dist2); + return ldist2 < max_dist2 ? GEN_min(dist2, closest_points(a_ltree, pack, max_dist2, pa, pb)) : dist2; + } + } +} + + +template <typename Shape1, typename Shape2> +MT_Scalar closest_points(const DT_BBoxTree& a, const DT_BBoxTree& b, const DT_DuoPack<Shape1, Shape2>& pack, + MT_Scalar max_dist2, MT_Point3& pa, MT_Point3& pb) +{ + if (a.m_type == DT_BBoxTree::LEAF && b.m_type == DT_BBoxTree::LEAF) + { + return closest_points(pack, a.m_index, b.m_index, max_dist2, pa, pb); + } + else if (a.m_type == DT_BBoxTree::LEAF || + (b.m_type != DT_BBoxTree::LEAF && a.m_cbox.size() < b.m_cbox.size())) + { + DT_BBoxTree b_ltree, b_rtree; + pack.m_b.m_nodes[b.m_index].makeChildren(pack.m_b.m_added, b_ltree, b_rtree); + MT_Scalar ldist2 = distance2(a.m_cbox, pack.m_a.m_xform, b_ltree.m_cbox, pack.m_b.m_xform); + MT_Scalar rdist2 = distance2(a.m_cbox, pack.m_a.m_xform, b_rtree.m_cbox, pack.m_b.m_xform); + if (ldist2 < rdist2) + { + MT_Scalar dist2 = ldist2 < max_dist2 ? closest_points(a, b_ltree, pack, max_dist2, pa, pb): MT_INFINITY;; + GEN_set_min(max_dist2, dist2); + return rdist2 < max_dist2 ? GEN_min(dist2, closest_points(a, b_rtree, pack, max_dist2, pa, pb)) : dist2; + } + else + { + MT_Scalar dist2 = rdist2 < max_dist2 ? closest_points(a, b_rtree, pack, max_dist2, pa, pb) : MT_INFINITY;; + GEN_set_min(max_dist2, dist2); + return ldist2 < max_dist2 ? GEN_min(dist2, closest_points(a, b_ltree, pack, max_dist2, pa, pb)) : dist2; + } + } + else + { + DT_BBoxTree a_ltree, a_rtree; + pack.m_a.m_nodes[a.m_index].makeChildren(pack.m_a.m_added, a_ltree, a_rtree); + MT_Scalar ldist2 = distance2(a_ltree.m_cbox, pack.m_a.m_xform, b.m_cbox, pack.m_b.m_xform); + MT_Scalar rdist2 = distance2(a_rtree.m_cbox, pack.m_a.m_xform, b.m_cbox, pack.m_b.m_xform); + if (ldist2 < rdist2) + { + MT_Scalar dist2 = ldist2 < max_dist2 ? closest_points(a_ltree, b, pack, max_dist2, pa, pb) : MT_INFINITY;; + GEN_set_min(max_dist2, dist2); + return rdist2 < max_dist2 ? GEN_min(dist2,closest_points(a_rtree, b, pack, max_dist2, pa, pb)) : dist2; + } + else + { + MT_Scalar dist2 = rdist2 < max_dist2 ? closest_points(a_rtree, b, pack, max_dist2, pa, pb) : MT_INFINITY; + GEN_set_min(max_dist2, dist2); + return ldist2 < max_dist2 ? GEN_min(dist2, closest_points(a_ltree, b, pack, max_dist2, pa, pb)) : dist2; + } + } +} + +#endif + diff --git a/extern/solid/src/complex/DT_CBox.h b/extern/solid/src/complex/DT_CBox.h new file mode 100755 index 00000000000..7fc7c5df4db --- /dev/null +++ b/extern/solid/src/complex/DT_CBox.h @@ -0,0 +1,134 @@ +/* + * 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. + */ + +#ifndef DT_CBOX_H +#define DT_CBOX_H + +#include "MT_BBox.h" + +struct DT_CBox { + DT_CBox() {} + DT_CBox(const MT_Point3& center, const MT_Vector3& extent) + : m_center(center), + m_extent(extent) + {} + + explicit DT_CBox(const MT_BBox& bbox) { set(bbox); } + + const MT_Point3& getCenter() const { return m_center; } + const MT_Vector3& getExtent() const { return m_extent; } + + void set(const MT_BBox& bbox) + { + m_center = bbox.getCenter(); + m_extent = bbox.getExtent(); + } + + MT_BBox get() const + { + return MT_BBox(m_center - m_extent, m_center + m_extent); + } + + MT_Scalar size() const + { + return GEN_max(GEN_max(m_extent[0], m_extent[1]), m_extent[2]); + } + + + DT_CBox& operator+=(const DT_CBox& box) + { + m_center += box.getCenter(); + m_extent += box.getExtent(); + return *this; + } + + int longestAxis() const { return m_extent.closestAxis(); } + + DT_CBox hull(const DT_CBox& b) const + { + return DT_CBox(this->get().hull(b.get())); + } + + bool overlaps(const DT_CBox& b) const + { + return MT_abs(m_center[0] - b.m_center[0]) <= m_extent[0] + b.m_extent[0] && + MT_abs(m_center[1] - b.m_center[1]) <= m_extent[1] + b.m_extent[1] && + MT_abs(m_center[2] - b.m_center[2]) <= m_extent[2] + b.m_extent[2]; + } + + bool overlapsLineSegment(const MT_Point3& p, const MT_Point3& q) const + { + MT_Vector3 r = q - p; + MT_Vector3 r_abs = r.absolute(); + + if (!overlaps(DT_CBox(p + r * MT_Scalar(0.5), r_abs * MT_Scalar(0.5)))) + { + return false; + } + + MT_Vector3 s = p - m_center; + + if (MT_abs(r[2] * s[1] - r[1] * s[2]) > r_abs[2] * m_extent[1] + r_abs[1] * m_extent[2]) + { + return false; + } + + if (MT_abs(r[0] * s[2] - r[2] * s[0]) > r_abs[0] * m_extent[2] + r_abs[2] * m_extent[0]) + { + return false; + } + + if (MT_abs(r[1] * s[0] - r[0] * s[1]) > r_abs[1] * m_extent[0] + r_abs[0] * m_extent[1]) + { + return false; + } + + return true; + } + + MT_Point3 support(const MT_Vector3& v) const + { + return m_center + MT_Vector3(v[0] < MT_Scalar(0.0) ? -m_extent[0] : m_extent[0], + v[1] < MT_Scalar(0.0) ? -m_extent[1] : m_extent[1], + v[2] < MT_Scalar(0.0) ? -m_extent[2] : m_extent[2]); + + } + +private: + MT_Point3 m_center; + MT_Vector3 m_extent; +}; + +inline DT_CBox operator+(const DT_CBox& b1, const DT_CBox& b2) +{ + return DT_CBox(b1.getCenter() + b2.getCenter(), + b1.getExtent() + b2.getExtent()); +} + +inline DT_CBox operator-(const DT_CBox& b1, const DT_CBox& b2) +{ + return DT_CBox(b1.getCenter() - b2.getCenter(), + b1.getExtent() + b2.getExtent()); +} + +#endif diff --git a/extern/solid/src/complex/DT_Complex.cpp b/extern/solid/src/complex/DT_Complex.cpp new file mode 100755 index 00000000000..023383a8427 --- /dev/null +++ b/extern/solid/src/complex/DT_Complex.cpp @@ -0,0 +1,327 @@ +/* + * 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 <new> +#include <fstream> + +#include "DT_Complex.h" +#include "DT_Minkowski.h" +#include "DT_Sphere.h" +#include "DT_Transform.h" + +DT_Complex::DT_Complex(const DT_VertexBase *base) + : m_base(base), + m_count(0), + m_leaves(0), + m_nodes(0) +{ + assert(base); + base->addComplex(this); +} + + +DT_Complex::~DT_Complex() +{ + DT_Index i; + for (i = 0; i != m_count; ++i) + { + delete m_leaves[i]; + } + delete [] m_leaves; + delete [] m_nodes; + + m_base->removeComplex(this); + if (m_base->isOwner()) + { + delete m_base; + } +} + +void DT_Complex::finish(DT_Count n, const DT_Convex *p[]) +{ + m_count = n; + + + assert(n >= 1); + + m_leaves = new const DT_Convex *[n]; + assert(m_leaves); + + DT_CBox *boxes = new DT_CBox[n]; + DT_Index *indices = new DT_Index[n]; + assert(boxes); + + DT_Index i; + for (i = 0; i != n; ++i) + { + m_leaves[i] = p[i]; + boxes[i].set(p[i]->bbox()); + indices[i] = i; + } + + m_cbox = boxes[0]; + for (i = 1; i != n; ++i) + { + m_cbox = m_cbox.hull(boxes[i]); + } + + if (n == 1) + { + m_nodes = 0; + m_type = DT_BBoxTree::LEAF; + } + else + { + m_nodes = new DT_BBoxNode[n - 1]; + assert(m_nodes); + + int num_nodes = 0; + new(&m_nodes[num_nodes++]) DT_BBoxNode(0, n, num_nodes, m_nodes, boxes, indices, m_cbox); + + assert(num_nodes == n - 1); + + m_type = DT_BBoxTree::INTERNAL; + } + + delete [] boxes; +} + + +MT_BBox DT_Complex::bbox(const MT_Transform& t, MT_Scalar margin) const +{ + MT_Matrix3x3 abs_b = t.getBasis().absolute(); + MT_Point3 center = t(m_cbox.getCenter()); + MT_Vector3 extent(margin + abs_b[0].dot(m_cbox.getExtent()), + margin + abs_b[1].dot(m_cbox.getExtent()), + margin + abs_b[2].dot(m_cbox.getExtent())); + + return MT_BBox(center - extent, center + extent); +} + +inline DT_CBox computeCBox(const DT_Convex *p) +{ + return DT_CBox(p->bbox()); +} + +inline DT_CBox computeCBox(MT_Scalar margin, const MT_Transform& xform) +{ + const MT_Matrix3x3& basis = xform.getBasis(); + return DT_CBox(MT_Point3(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)), + MT_Vector3(basis[0].length() * margin, + basis[1].length() * margin, + basis[2].length() * margin)); +} + +void DT_Complex::refit() +{ + DT_RootData<const DT_Convex *> rd(m_nodes, m_leaves); + DT_Index i = m_count - 1; + while (i--) + { + ::refit(m_nodes[i], rd); + } + m_cbox = m_type == DT_BBoxTree::LEAF ? computeCBox(m_leaves[0]) : m_nodes[0].hull(); +} + +inline bool ray_cast(const DT_RootData<const DT_Convex *>& rd, DT_Index index, const MT_Point3& source, const MT_Point3& target, + MT_Scalar& lambda, MT_Vector3& normal) +{ + return rd.m_leaves[index]->ray_cast(source, target, lambda, normal); +} + +bool DT_Complex::ray_cast(const MT_Point3& source, const MT_Point3& target, + MT_Scalar& lambda, MT_Vector3& normal) const +{ + DT_RootData<const DT_Convex *> rd(m_nodes, m_leaves); + + return ::ray_cast(DT_BBoxTree(m_cbox, 0, m_type), rd, source, target, lambda, normal); +} + +inline bool intersect(const DT_Pack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, MT_Vector3& v) +{ + DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]); + MT_Scalar a_margin = pack.m_a.m_plus; + return ::intersect((a_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : + static_cast<const DT_Convex&>(ta)), + pack.m_b, v); +} + +bool intersect(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Convex& b, MT_Vector3& v) +{ + DT_Pack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), b); + + return intersect(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), pack, v); +} + +inline bool intersect(const DT_DuoPack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, DT_Index b_index, MT_Vector3& v) +{ + DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]); + MT_Scalar a_margin = pack.m_a.m_plus; + DT_Transform tb = DT_Transform(pack.m_b.m_xform, *pack.m_b.m_leaves[b_index]); + MT_Scalar b_margin = pack.m_b.m_plus; + return ::intersect((a_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : + static_cast<const DT_Convex&>(ta)), + (b_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : + static_cast<const DT_Convex&>(tb)), + v); +} + +bool intersect(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Complex& b, const MT_Transform& b2w, MT_Scalar b_margin, MT_Vector3& v) +{ + DT_DuoPack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), + DT_ObjectData<const DT_Convex *, MT_Scalar>(b.m_nodes, b.m_leaves, b2w, b_margin)); + + + return intersect(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), + DT_BBoxTree(b.m_cbox + pack.m_b.m_added, 0, b.m_type), pack, v); +} + +inline bool common_point(const DT_Pack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]); + MT_Scalar a_margin = pack.m_a.m_plus; + return ::common_point((a_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : + static_cast<const DT_Convex&>(ta)), + pack.m_b, v, pa, pb); +} + +bool common_point(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Convex& b, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + DT_Pack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), b); + + return common_point(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), pack, v, pb, pa); +} + +inline bool common_point(const DT_DuoPack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, DT_Index b_index, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]); + MT_Scalar a_margin = pack.m_a.m_plus; + DT_Transform tb = DT_Transform(pack.m_b.m_xform, *pack.m_b.m_leaves[b_index]); + MT_Scalar b_margin = pack.m_b.m_plus; + return ::common_point((a_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : + static_cast<const DT_Convex&>(ta)), + (b_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : + static_cast<const DT_Convex&>(tb)), + v, pa, pb); +} + +bool common_point(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Complex& b, const MT_Transform& b2w, MT_Scalar b_margin, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + DT_DuoPack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), + DT_ObjectData<const DT_Convex *, MT_Scalar>(b.m_nodes, b.m_leaves, b2w, b_margin)); + + return common_point(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), + DT_BBoxTree(b.m_cbox + pack.m_b.m_added, 0, b.m_type), pack, v, pa, pb); +} + +inline bool penetration_depth(const DT_HybridPack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]); + return ::hybrid_penetration_depth(ta, pack.m_a.m_plus, pack.m_b, pack.m_margin, v, pa, pb); +} + +bool penetration_depth(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Convex& b, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + DT_HybridPack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), b, b_margin); + + MT_Scalar max_pen_len = MT_Scalar(0.0); + return penetration_depth(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), pack, v, pa, pb, max_pen_len); +} + +inline bool penetration_depth(const DT_DuoPack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, DT_Index b_index, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]); + DT_Transform tb = DT_Transform(pack.m_b.m_xform, *pack.m_b.m_leaves[b_index]); + return ::hybrid_penetration_depth(ta, pack.m_a.m_plus, tb, pack.m_a.m_plus, v, pa, pb); +} + +bool penetration_depth(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Complex& b, const MT_Transform& b2w, MT_Scalar b_margin, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + DT_DuoPack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), + DT_ObjectData<const DT_Convex *, MT_Scalar>(b.m_nodes, b.m_leaves, b2w, b_margin)); + + MT_Scalar max_pen_len = MT_Scalar(0.0); + return penetration_depth(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), + DT_BBoxTree(b.m_cbox + pack.m_b.m_added, 0, b.m_type), pack, v, pa, pb, max_pen_len); +} + + + +inline MT_Scalar closest_points(const DT_Pack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, MT_Scalar max_dist2, MT_Point3& pa, MT_Point3& pb) +{ + DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]); + MT_Scalar a_margin = pack.m_a.m_plus; + return ::closest_points((a_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : + static_cast<const DT_Convex&>(ta)), + pack.m_b, max_dist2, pa, pb); +} + +MT_Scalar closest_points(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Convex& b, MT_Point3& pa, MT_Point3& pb) +{ + DT_Pack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), b); + + return closest_points(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), pack, MT_INFINITY, pa, pb); +} + +inline MT_Scalar closest_points(const DT_DuoPack<const DT_Convex *, MT_Scalar>& pack, DT_Index a_index, DT_Index b_index, MT_Scalar max_dist2, MT_Point3& pa, MT_Point3& pb) +{ + DT_Transform ta = DT_Transform(pack.m_a.m_xform, *pack.m_a.m_leaves[a_index]); + MT_Scalar a_margin = pack.m_a.m_plus; + DT_Transform tb = DT_Transform(pack.m_b.m_xform, *pack.m_b.m_leaves[b_index]); + MT_Scalar b_margin = pack.m_b.m_plus; + return ::closest_points((a_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(ta, DT_Sphere(a_margin))) : + static_cast<const DT_Convex&>(ta)), + (b_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(tb, DT_Sphere(b_margin))) : + static_cast<const DT_Convex&>(tb)), max_dist2, pa, pb); +} + +MT_Scalar closest_points(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Complex& b, const MT_Transform& b2w, MT_Scalar b_margin, + MT_Point3& pa, MT_Point3& pb) +{ + DT_DuoPack<const DT_Convex *, MT_Scalar> pack(DT_ObjectData<const DT_Convex *, MT_Scalar>(a.m_nodes, a.m_leaves, a2w, a_margin), + DT_ObjectData<const DT_Convex *, MT_Scalar>(b.m_nodes, b.m_leaves, b2w, b_margin)); + + return closest_points(DT_BBoxTree(a.m_cbox + pack.m_a.m_added, 0, a.m_type), + DT_BBoxTree(b.m_cbox + pack.m_b.m_added, 0, b.m_type), pack, MT_INFINITY, pa, pb); +} + + diff --git a/extern/solid/src/complex/DT_Complex.h b/extern/solid/src/complex/DT_Complex.h new file mode 100755 index 00000000000..ae08a05d4c9 --- /dev/null +++ b/extern/solid/src/complex/DT_Complex.h @@ -0,0 +1,94 @@ +/* + * 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. + */ + +#ifndef DT_COMPLEX_H +#define DT_COMPLEX_H + +#include <algorithm> + +#include "MT_Transform.h" +#include "DT_VertexBase.h" + +#include "DT_Shape.h" +#include "DT_CBox.h" +#include "DT_BBoxTree.h" + +class DT_Convex; + +class DT_Complex : public DT_Shape { +public: + DT_Complex(const DT_VertexBase *base); + virtual ~DT_Complex(); + + void finish(DT_Count n, const DT_Convex *p[]); + + virtual DT_ShapeType getType() const { return COMPLEX; } + + virtual MT_BBox bbox(const MT_Transform& t, MT_Scalar margin) const; + + virtual bool ray_cast(const MT_Point3& source, const MT_Point3& target, + MT_Scalar& lambda, MT_Vector3& normal) const; + + void refit(); + + + friend bool intersect(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Convex& b, MT_Vector3& v); + + friend bool intersect(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Complex& b, const MT_Transform& b2w, MT_Scalar b_margin, + MT_Vector3& v); + + friend bool common_point(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Convex& b, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb); + + friend bool common_point(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Complex& b, const MT_Transform& b2w, MT_Scalar b_margin, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb); + + friend bool penetration_depth(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Convex& b, MT_Scalar b_margin, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb); + + friend bool penetration_depth(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Complex& b, const MT_Transform& b2w, MT_Scalar b_margin, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb); + + friend MT_Scalar closest_points(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Convex& b, MT_Point3& pa, MT_Point3& pb); + + friend MT_Scalar closest_points(const DT_Complex& a, const MT_Transform& a2w, MT_Scalar a_margin, + const DT_Complex& b, const MT_Transform& b2w, MT_Scalar b_margin, + MT_Point3& pa, MT_Point3& pb); + + const DT_VertexBase *m_base; + DT_Count m_count; + const DT_Convex **m_leaves; + DT_BBoxNode *m_nodes; + DT_CBox m_cbox; + DT_BBoxTree::NodeType m_type; +}; + +#endif + + + diff --git a/extern/solid/src/convex/DT_Accuracy.cpp b/extern/solid/src/convex/DT_Accuracy.cpp new file mode 100755 index 00000000000..113275b0dbd --- /dev/null +++ b/extern/solid/src/convex/DT_Accuracy.cpp @@ -0,0 +1,30 @@ +/* + * 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_Accuracy.h" + +static const MT_Scalar rel_error = MT_Scalar(1.0e-3); + +MT_Scalar DT_Accuracy::rel_error2 = rel_error * rel_error; +MT_Scalar DT_Accuracy::depth_tolerance = MT_Scalar(1.0) + MT_Scalar(2.0) * rel_error; +MT_Scalar DT_Accuracy::tol_error = MT_EPSILON; diff --git a/extern/solid/src/convex/DT_Accuracy.h b/extern/solid/src/convex/DT_Accuracy.h new file mode 100755 index 00000000000..9759a6fd4c5 --- /dev/null +++ b/extern/solid/src/convex/DT_Accuracy.h @@ -0,0 +1,47 @@ +/* + * 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. + */ + +#ifndef DT_ACCURACY_H +#define DT_ACCURACY_H + +#include "MT_Scalar.h" + +class DT_Accuracy { +public: + static MT_Scalar rel_error2; // squared relative error in the computed distance + static MT_Scalar depth_tolerance; // terminate EPA if upper_bound <= depth_tolerance * dist2 + static MT_Scalar tol_error; // error tolerance if the distance is almost zero + + static void setAccuracy(MT_Scalar rel_error) + { + rel_error2 = rel_error * rel_error; + depth_tolerance = MT_Scalar(1.0) + MT_Scalar(2.0) * rel_error; + } + + static void setTolerance(MT_Scalar epsilon) + { + tol_error = epsilon; + } +}; + +#endif diff --git a/extern/solid/src/convex/DT_Array.h b/extern/solid/src/convex/DT_Array.h new file mode 100755 index 00000000000..603ebe978f1 --- /dev/null +++ b/extern/solid/src/convex/DT_Array.h @@ -0,0 +1,71 @@ +/* + * 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. + */ + +#ifndef DT_ARRAY_H +#define DT_ARRAY_H + +#include <cassert> + +template <typename Data, typename Size = size_t> +class DT_Array { +public: + DT_Array() + : m_count(0), + m_data(0) + {} + + explicit DT_Array(Size count) + : m_count(count), + m_data(new Data[count]) + { + assert(m_data); + } + + DT_Array(Size count, const Data *data) + : m_count(count), + m_data(new Data[count]) + { + assert(m_data); + std::copy(&data[0], &data[count], m_data); + } + + ~DT_Array() + { + delete [] m_data; + } + + const Data& operator[](int i) const { return m_data[i]; } + Data& operator[](int i) { return m_data[i]; } + + Size size() const { return m_count; } + +private: + DT_Array(const DT_Array&); + DT_Array& operator=(const DT_Array&); + + Size m_count; + Data *m_data; +}; + +#endif + diff --git a/extern/solid/src/convex/DT_Box.cpp b/extern/solid/src/convex/DT_Box.cpp new file mode 100755 index 00000000000..0b46f566fe8 --- /dev/null +++ b/extern/solid/src/convex/DT_Box.cpp @@ -0,0 +1,112 @@ +/* + * 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_Box.h" + +MT_Scalar DT_Box::supportH(const MT_Vector3& v) const +{ + return v.absolute().dot(m_extent); +} + +MT_Point3 DT_Box::support(const MT_Vector3& v) const +{ + return MT_Point3(v[0] < MT_Scalar(0.0) ? -m_extent[0] : m_extent[0], + v[1] < MT_Scalar(0.0) ? -m_extent[1] : m_extent[1], + v[2] < MT_Scalar(0.0) ? -m_extent[2] : m_extent[2]); + +} + + +bool DT_Box::ray_cast(const MT_Point3& source, const MT_Point3& target, + MT_Scalar& param, MT_Vector3& normal) const +{ + T_Outcode source_bits = outcode(source); + T_Outcode target_bits = outcode(target); + + if ((source_bits & target_bits) == 0x0) + // None of the side planes separate the ray from the box. + { + MT_Scalar lambda_enter = MT_Scalar(0.0); + MT_Scalar lambda_exit = param; + MT_Vector3 r = target - source; + T_Outcode normal_bit = 0x0; // Indicates the axis that is returned as normal. + T_Outcode bit = 0x01; + int i; + for (i = 0; i != 3; ++i) + { + if (source_bits & bit) + // Point of intersection is entering + { + MT_Scalar lambda = (-source[i] - m_extent[i]) / r[i]; + if (lambda_enter < lambda) + { + lambda_enter = lambda; + normal_bit = bit; + } + } + else if (target_bits & bit) + // Point of intersection is exiting + { + MT_Scalar lambda = (-source[i] - m_extent[i]) / r[i]; + GEN_set_min(lambda_exit, lambda); + } + bit <<=1; + if (source_bits & bit) + // Point of intersection is entering + { + MT_Scalar lambda = (-source[i] + m_extent[i]) / r[i]; + if (lambda_enter < lambda) + { + lambda_enter = lambda; + normal_bit = bit; + } + } + else if (target_bits & bit) + // Point of intersection is exiting + { + MT_Scalar lambda = (-source[i] + m_extent[i]) / r[i]; + GEN_set_min(lambda_exit, lambda); + } + bit <<=1; + } + if (lambda_enter <= lambda_exit) + // The ray intersects the box + { + param = lambda_enter; + normal.setValue(normal_bit == 0x01 ? -MT_Scalar(1.0) : + normal_bit == 0x02 ? MT_Scalar(1.0) : + MT_Scalar(0.0), + normal_bit == 0x04 ? -MT_Scalar(1.0) : + normal_bit == 0x08 ? MT_Scalar(1.0) : + MT_Scalar(0.0), + normal_bit == 0x10 ? -MT_Scalar(1.0) : + normal_bit == 0x20 ? MT_Scalar(1.0) : + MT_Scalar(0.0)); + return true; + } + } + + return false; +} + + diff --git a/extern/solid/src/convex/DT_Box.h b/extern/solid/src/convex/DT_Box.h new file mode 100755 index 00000000000..ace9634b5e3 --- /dev/null +++ b/extern/solid/src/convex/DT_Box.h @@ -0,0 +1,62 @@ +/* + * 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. + */ + +#ifndef DT_BOX_H +#define DT_BOX_H + +#include "DT_Convex.h" + +class DT_Box : public DT_Convex { +public: + DT_Box(MT_Scalar x, MT_Scalar y, MT_Scalar z) : + m_extent(x, y, z) + {} + + DT_Box(const MT_Vector3& e) : + m_extent(e) + {} + + virtual MT_Scalar supportH(const MT_Vector3& v) const; + virtual MT_Point3 support(const MT_Vector3& v) const; + virtual bool ray_cast(const MT_Point3& source, const MT_Point3& target, + MT_Scalar& param, MT_Vector3& normal) const; + + const MT_Vector3& getExtent() const { return m_extent; } + +protected: + typedef unsigned int T_Outcode; + + T_Outcode outcode(const MT_Point3& p) const + { + return (p[0] < -m_extent[0] ? 0x01 : 0x0) | + (p[0] > m_extent[0] ? 0x02 : 0x0) | + (p[1] < -m_extent[1] ? 0x04 : 0x0) | + (p[1] > m_extent[1] ? 0x08 : 0x0) | + (p[2] < -m_extent[2] ? 0x10 : 0x0) | + (p[2] > m_extent[2] ? 0x20 : 0x0); + } + + MT_Vector3 m_extent; +}; + +#endif diff --git a/extern/solid/src/convex/DT_Cone.cpp b/extern/solid/src/convex/DT_Cone.cpp new file mode 100755 index 00000000000..1dd6a7ddbbe --- /dev/null +++ b/extern/solid/src/convex/DT_Cone.cpp @@ -0,0 +1,48 @@ +/* + * 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_Cone.h" + +MT_Point3 DT_Cone::support(const MT_Vector3& v) const +{ + MT_Scalar v_len = v.length(); + + if (v[1] > v_len * sinAngle) + { + return MT_Point3(MT_Scalar(0.0), halfHeight, MT_Scalar(0.0)); + } + else + { + MT_Scalar s = MT_sqrt(v[0] * v[0] + v[2] * v[2]); + if (s != MT_Scalar(0.0)) + { + MT_Scalar d = bottomRadius / s; + return MT_Point3(v[0] * d, -halfHeight, v[2] * d); + } + else + { + return MT_Point3(bottomRadius, -halfHeight, MT_Scalar(0.0)); + } + } +} + diff --git a/extern/solid/src/convex/DT_Cone.h b/extern/solid/src/convex/DT_Cone.h new file mode 100755 index 00000000000..85e416877dd --- /dev/null +++ b/extern/solid/src/convex/DT_Cone.h @@ -0,0 +1,45 @@ +/* + * 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. + */ + +#ifndef DT_CONE_H +#define DT_CONE_H + +#include "DT_Convex.h" + +class DT_Cone : public DT_Convex { +public: + DT_Cone(MT_Scalar r, MT_Scalar h) : + bottomRadius(r), + halfHeight(h * MT_Scalar(0.5)), + sinAngle(r / MT_sqrt(r * r + h * h)) + {} + + virtual MT_Point3 support(const MT_Vector3& v) const; + +protected: + MT_Scalar bottomRadius; + MT_Scalar halfHeight; + MT_Scalar sinAngle; +}; + +#endif diff --git a/extern/solid/src/convex/DT_Convex.cpp b/extern/solid/src/convex/DT_Convex.cpp new file mode 100755 index 00000000000..3be47f6ed02 --- /dev/null +++ b/extern/solid/src/convex/DT_Convex.cpp @@ -0,0 +1,426 @@ +/* + * 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_Convex.h" +#include "GEN_MinMax.h" + +//#define DEBUG +#define SAFE_EXIT + +#include "DT_GJK.h" +#include "DT_PenDepth.h" + +#include <algorithm> +#include <new> + +#include "MT_BBox.h" +#include "DT_Sphere.h" +#include "DT_Minkowski.h" + +#include "DT_Accuracy.h" + +#ifdef STATISTICS +int num_iterations = 0; +int num_irregularities = 0; +#endif + +MT_BBox DT_Convex::bbox() const +{ + MT_Point3 min(-supportH(MT_Vector3(-1.0f, 0.0f, 0.0f)), + -supportH(MT_Vector3(0.0f, -1.0f, 0.0f)), + -supportH(MT_Vector3(0.0f, 0.0f, -1.0f))); + MT_Point3 max( supportH(MT_Vector3(1.0f, 0.0f, 0.0f)), + supportH(MT_Vector3(0.0f, 1.0f, 0.0f)), + supportH(MT_Vector3(0.0f, 0.0f, 1.0f))); + + + return MT_BBox(min, max); +} + +MT_BBox DT_Convex::bbox(const MT_Matrix3x3& basis) const +{ + MT_Point3 min(-supportH(-basis[0]), + -supportH(-basis[1]), + -supportH(-basis[2])); + MT_Point3 max( supportH( basis[0]), + supportH( basis[1]), + supportH( basis[2])); + return MT_BBox(min, max); +} + +MT_BBox DT_Convex::bbox(const MT_Transform& t, MT_Scalar margin) const +{ + MT_Point3 min(t.getOrigin()[0] - supportH(-t.getBasis()[0]) - margin, + t.getOrigin()[1] - supportH(-t.getBasis()[1]) - margin, + t.getOrigin()[2] - supportH(-t.getBasis()[2]) - margin); + MT_Point3 max(t.getOrigin()[0] + supportH( t.getBasis()[0]) + margin, + t.getOrigin()[1] + supportH( t.getBasis()[1]) + margin, + t.getOrigin()[2] + supportH( t.getBasis()[2]) + margin); + return MT_BBox(min, max); +} + +bool DT_Convex::ray_cast(const MT_Point3& source, const MT_Point3& target, MT_Scalar& lambda, MT_Vector3& normal) const +{ + // Still working on this one... + return false; +} + +bool intersect(const DT_Convex& a, const DT_Convex& b, MT_Vector3& v) +{ + DT_GJK gjk; + +#ifdef STATISTICS + num_iterations = 0; +#endif + MT_Scalar dist2 = MT_INFINITY; + + do + { + MT_Point3 p = a.support(-v); + MT_Point3 q = b.support(v); + MT_Vector3 w = p - q; + + if (v.dot(w) > MT_Scalar(0.0)) + { + return false; + } + + gjk.addVertex(w); + +#ifdef STATISTICS + ++num_iterations; +#endif + if (!gjk.closest(v)) + { +#ifdef STATISTICS + ++num_irregularities; +#endif + return false; + } + +#ifdef SAFE_EXIT + MT_Scalar prev_dist2 = dist2; +#endif + + dist2 = v.length2(); + +#ifdef SAFE_EXIT + if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) + { + return false; + } +#endif + } + while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); + + v.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); + + return true; +} + + + + +bool common_point(const DT_Convex& a, const DT_Convex& b, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + DT_GJK gjk; + +#ifdef STATISTICS + num_iterations = 0; +#endif + + MT_Scalar dist2 = MT_INFINITY; + + do + { + MT_Point3 p = a.support(-v); + MT_Point3 q = b.support(v); + MT_Vector3 w = p - q; + + if (v.dot(w) > MT_Scalar(0.0)) + { + return false; + } + + gjk.addVertex(w, p, q); + +#ifdef STATISTICS + ++num_iterations; +#endif + if (!gjk.closest(v)) + { +#ifdef STATISTICS + ++num_irregularities; +#endif + return false; + } + +#ifdef SAFE_EXIT + MT_Scalar prev_dist2 = dist2; +#endif + + dist2 = v.length2(); + +#ifdef SAFE_EXIT + if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) + { + return false; + } +#endif + } + while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); + + gjk.compute_points(pa, pb); + + v.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); + + return true; +} + + + + + + + +bool penetration_depth(const DT_Convex& a, const DT_Convex& b, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + DT_GJK gjk; + +#ifdef STATISTICS + num_iterations = 0; +#endif + + MT_Scalar dist2 = MT_INFINITY; + + do + { + MT_Point3 p = a.support(-v); + MT_Point3 q = b.support(v); + MT_Vector3 w = p - q; + + if (v.dot(w) > MT_Scalar(0.0)) + { + return false; + } + + gjk.addVertex(w, p, q); + +#ifdef STATISTICS + ++num_iterations; +#endif + if (!gjk.closest(v)) + { +#ifdef STATISTICS + ++num_irregularities; +#endif + return false; + } + +#ifdef SAFE_EXIT + MT_Scalar prev_dist2 = dist2; +#endif + + dist2 = v.length2(); + +#ifdef SAFE_EXIT + if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) + { + return false; + } +#endif + } + while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); + + + return penDepth(gjk, a, b, v, pa, pb); + +} + +bool hybrid_penetration_depth(const DT_Convex& a, MT_Scalar a_margin, + const DT_Convex& b, MT_Scalar b_margin, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + MT_Scalar margin = a_margin + b_margin; + if (margin > MT_Scalar(0.0)) + { + MT_Scalar margin2 = margin * margin; + + DT_GJK gjk; + +#ifdef STATISTICS + num_iterations = 0; +#endif + MT_Scalar dist2 = MT_INFINITY; + + do + { + MT_Point3 p = a.support(-v); + MT_Point3 q = b.support(v); + + MT_Vector3 w = p - q; + + MT_Scalar delta = v.dot(w); + + if (delta > MT_Scalar(0.0) && delta * delta > dist2 * margin2) + { + return false; + } + + if (gjk.inSimplex(w) || dist2 - delta <= dist2 * DT_Accuracy::rel_error2) + { + gjk.compute_points(pa, pb); + MT_Scalar s = MT_sqrt(dist2); + assert(s > MT_Scalar(0.0)); + pa -= v * (a_margin / s); + pb += v * (b_margin / s); + return true; + } + + gjk.addVertex(w, p, q); + +#ifdef STATISTICS + ++num_iterations; +#endif + if (!gjk.closest(v)) + { +#ifdef STATISTICS + ++num_irregularities; +#endif + gjk.compute_points(pa, pb); + MT_Scalar s = MT_sqrt(dist2); + assert(s > MT_Scalar(0.0)); + pa -= v * (a_margin / s); + pb += v * (b_margin / s); + return true; + } + +#ifdef SAFE_EXIT + MT_Scalar prev_dist2 = dist2; +#endif + + dist2 = v.length2(); + +#ifdef SAFE_EXIT + if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) + { + gjk.backup_closest(v); + dist2 = v.length2(); + gjk.compute_points(pa, pb); + MT_Scalar s = MT_sqrt(dist2); + assert(s > MT_Scalar(0.0)); + pa -= v * (a_margin / s); + pb += v * (b_margin / s); + return true; + } +#endif + } + while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); + + } + // Second GJK phase. compute points on the boundary of the offset object + + return penetration_depth((a_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(a, DT_Sphere(a_margin))) : + static_cast<const DT_Convex&>(a)), + (b_margin > MT_Scalar(0.0) ? + static_cast<const DT_Convex&>(DT_Minkowski(b, DT_Sphere(b_margin))) : + static_cast<const DT_Convex&>(b)), v, pa, pb); +} + + +MT_Scalar closest_points(const DT_Convex& a, const DT_Convex& b, MT_Scalar max_dist2, + MT_Point3& pa, MT_Point3& pb) +{ + MT_Vector3 v(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); + + DT_GJK gjk; + +#ifdef STATISTICS + num_iterations = 0; +#endif + + MT_Scalar dist2 = MT_INFINITY; + + do + { + MT_Point3 p = a.support(-v); + MT_Point3 q = b.support(v); + MT_Vector3 w = p - q; + + MT_Scalar delta = v.dot(w); + if (delta > MT_Scalar(0.0) && delta * delta > dist2 * max_dist2) + { + return MT_INFINITY; + } + + if (gjk.inSimplex(w) || dist2 - delta <= dist2 * DT_Accuracy::rel_error2) + { + break; + } + + gjk.addVertex(w, p, q); + +#ifdef STATISTICS + ++num_iterations; + if (num_iterations > 1000) + { + std::cout << "v: " << v << " w: " << w << std::endl; + } +#endif + if (!gjk.closest(v)) + { +#ifdef STATISTICS + ++num_irregularities; +#endif + break; + } + +#ifdef SAFE_EXIT + MT_Scalar prev_dist2 = dist2; +#endif + + dist2 = v.length2(); + +#ifdef SAFE_EXIT + if (prev_dist2 - dist2 <= MT_EPSILON * prev_dist2) + { + gjk.backup_closest(v); + dist2 = v.length2(); + break; + } +#endif + } + while (!gjk.fullSimplex() && dist2 > DT_Accuracy::tol_error * gjk.maxVertex()); + + assert(!gjk.emptySimplex()); + + if (dist2 <= max_dist2) + { + gjk.compute_points(pa, pb); + } + + return dist2; +} diff --git a/extern/solid/src/convex/DT_Convex.h b/extern/solid/src/convex/DT_Convex.h new file mode 100755 index 00000000000..dd620ac8b98 --- /dev/null +++ b/extern/solid/src/convex/DT_Convex.h @@ -0,0 +1,64 @@ +/* + * 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. + */ + +#ifndef DT_CONVEX_H +#define DT_CONVEX_H + +#include "DT_Shape.h" + +#include "MT_Vector3.h" +#include "MT_Point3.h" + +#include "MT_Matrix3x3.h" +#include "MT_Transform.h" + +class DT_Convex : public DT_Shape { +public: + virtual ~DT_Convex() {} + virtual DT_ShapeType getType() const { return CONVEX; } + + virtual MT_Scalar supportH(const MT_Vector3& v) const { return v.dot(support(v)); } + virtual MT_Point3 support(const MT_Vector3& v) const = 0; + virtual MT_BBox bbox() const; + virtual MT_BBox bbox(const MT_Matrix3x3& basis) const; + virtual MT_BBox bbox(const MT_Transform& t, MT_Scalar margin = MT_Scalar(0.0)) const; + virtual bool ray_cast(const MT_Point3& source, const MT_Point3& target, MT_Scalar& param, MT_Vector3& normal) const; + +protected: + DT_Convex() {} +}; + + +bool intersect(const DT_Convex& a, const DT_Convex& b, MT_Vector3& v); + +bool common_point(const DT_Convex& a, const DT_Convex& b, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb); + +MT_Scalar closest_points(const DT_Convex&, const DT_Convex&, MT_Scalar max_dist2, MT_Point3& pa, MT_Point3& pb); + +bool penetration_depth(const DT_Convex& a, const DT_Convex& b, MT_Vector3& v, MT_Point3& pa, MT_Point3& pb); + +bool hybrid_penetration_depth(const DT_Convex& a, MT_Scalar a_margin, + const DT_Convex& b, MT_Scalar b_margin, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb); + +#endif diff --git a/extern/solid/src/convex/DT_Cylinder.cpp b/extern/solid/src/convex/DT_Cylinder.cpp new file mode 100755 index 00000000000..cff5ebcefb1 --- /dev/null +++ b/extern/solid/src/convex/DT_Cylinder.cpp @@ -0,0 +1,39 @@ +/* + * 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_Cylinder.h" + +MT_Point3 DT_Cylinder::support(const MT_Vector3& v) const +{ + MT_Scalar s = MT_sqrt(v[0] * v[0] + v[2] * v[2]); + if (s != MT_Scalar(0.0)) + { + MT_Scalar d = radius / s; + return MT_Point3(v[0] * d, v[1] < 0.0 ? -halfHeight : halfHeight, v[2] * d); + } + else + { + return MT_Point3(radius, v[1] < 0.0 ? -halfHeight : halfHeight, MT_Scalar(0.0)); + } +} + diff --git a/extern/solid/src/convex/DT_Cylinder.h b/extern/solid/src/convex/DT_Cylinder.h new file mode 100755 index 00000000000..2a0c07fd579 --- /dev/null +++ b/extern/solid/src/convex/DT_Cylinder.h @@ -0,0 +1,42 @@ +/* + * 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. + */ + +#ifndef DT_CYLINDER_H +#define DT_CYLINDER_H + +#include "DT_Convex.h" + +class DT_Cylinder : public DT_Convex { +public: + DT_Cylinder(MT_Scalar r, MT_Scalar h) : + radius(r), + halfHeight(h * MT_Scalar(0.5)) {} + + virtual MT_Point3 support(const MT_Vector3& v) const; + +protected: + MT_Scalar radius; + MT_Scalar halfHeight; +}; + +#endif diff --git a/extern/solid/src/convex/DT_Facet.cpp b/extern/solid/src/convex/DT_Facet.cpp new file mode 100755 index 00000000000..87ae5c3e0be --- /dev/null +++ b/extern/solid/src/convex/DT_Facet.cpp @@ -0,0 +1,80 @@ +/* + * 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_Facet.h" + +bool DT_Facet::link(int edge0, DT_Facet *facet, int edge1) +{ + m_adjFacets[edge0] = facet; + m_adjEdges[edge0] = edge1; + facet->m_adjFacets[edge1] = this; + facet->m_adjEdges[edge1] = edge0; + + bool b = m_indices[edge0] == facet->m_indices[incMod3(edge1)] && + m_indices[incMod3(edge0)] == facet->m_indices[edge1]; + return b; +} + +bool DT_Facet::computeClosest(const MT_Vector3 *verts) +{ + const MT_Vector3& p0 = verts[m_indices[0]]; + + MT_Vector3 v1 = verts[m_indices[1]] - p0; + MT_Vector3 v2 = verts[m_indices[2]] - p0; + MT_Scalar v1dv1 = v1.length2(); + MT_Scalar v1dv2 = v1.dot(v2); + MT_Scalar v2dv2 = v2.length2(); + MT_Scalar p0dv1 = p0.dot(v1); + MT_Scalar p0dv2 = p0.dot(v2); + + m_det = v1dv1 * v2dv2 - v1dv2 * v1dv2; // non-negative + m_lambda1 = p0dv2 * v1dv2 - p0dv1 * v2dv2; + m_lambda2 = p0dv1 * v1dv2 - p0dv2 * v1dv1; + + if (m_det > MT_Scalar(0.0)) { + m_closest = p0 + (m_lambda1 * v1 + m_lambda2 * v2) / m_det; + m_dist2 = m_closest.length2(); + return true; + } + + return false; +} + +void DT_Facet::silhouette(int index, const MT_Vector3& w, + DT_EdgeBuffer& edgeBuffer) +{ + if (!m_obsolete) { + if (m_closest.dot(w) < m_dist2) { + edgeBuffer.push_back(DT_Edge(this, index)); + } + else { + m_obsolete = true; // Facet is visible + int next = incMod3(index); + m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer); + next = incMod3(next); + m_adjFacets[next]->silhouette(m_adjEdges[next], w, edgeBuffer); + } + } +} + + diff --git a/extern/solid/src/convex/DT_Facet.h b/extern/solid/src/convex/DT_Facet.h new file mode 100755 index 00000000000..873706346b8 --- /dev/null +++ b/extern/solid/src/convex/DT_Facet.h @@ -0,0 +1,134 @@ +/* + * 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. + */ + +#ifndef DT_FACET_H +#define DT_FACET_H + +#include <string.h> +#include <vector> + +#include <MT_Vector3.h> +#include <MT_Point3.h> + +class DT_Facet; + + +class DT_Edge { +public: + DT_Edge() {} + DT_Edge(DT_Facet *facet, int index) : + m_facet(facet), + m_index(index) {} + + DT_Facet *getFacet() const { return m_facet; } + int getIndex() const { return m_index; } + + int getSource() const; + int getTarget() const; + +private: + DT_Facet *m_facet; + int m_index; +}; + +typedef std::vector<DT_Edge> DT_EdgeBuffer; + + +class DT_Facet { +public: + DT_Facet() {} + DT_Facet(int i0, int i1, int i2) + : m_obsolete(false) + { + m_indices[0] = i0; + m_indices[1] = i1; + m_indices[2] = i2; + } + + int operator[](int i) const { return m_indices[i]; } + + bool link(int edge0, DT_Facet *facet, int edge1); + + + bool isObsolete() const { return m_obsolete; } + + + bool computeClosest(const MT_Vector3 *verts); + + const MT_Vector3& getClosest() const { return m_closest; } + + bool isClosestInternal() const + { + return m_lambda1 >= MT_Scalar(0.0) && + m_lambda2 >= MT_Scalar(0.0) && + m_lambda1 + m_lambda2 <= m_det; + } + + MT_Scalar getDist2() const { return m_dist2; } + + MT_Point3 getClosestPoint(const MT_Point3 *points) const + { + const MT_Point3& p0 = points[m_indices[0]]; + + return p0 + (m_lambda1 * (points[m_indices[1]] - p0) + + m_lambda2 * (points[m_indices[2]] - p0)) / m_det; + } + + void silhouette(const MT_Vector3& w, DT_EdgeBuffer& edgeBuffer) + { + edgeBuffer.clear(); + m_obsolete = true; + m_adjFacets[0]->silhouette(m_adjEdges[0], w, edgeBuffer); + m_adjFacets[1]->silhouette(m_adjEdges[1], w, edgeBuffer); + m_adjFacets[2]->silhouette(m_adjEdges[2], w, edgeBuffer); + } + +private: + void silhouette(int index, const MT_Vector3& w, DT_EdgeBuffer& edgeBuffer); + + int m_indices[3]; + bool m_obsolete; + DT_Facet *m_adjFacets[3]; + int m_adjEdges[3]; + + MT_Scalar m_det; + MT_Scalar m_lambda1; + MT_Scalar m_lambda2; + MT_Vector3 m_closest; + MT_Scalar m_dist2; +}; + + +inline int incMod3(int i) { return ++i % 3; } + +inline int DT_Edge::getSource() const +{ + return (*m_facet)[m_index]; +} + +inline int DT_Edge::getTarget() const +{ + return (*m_facet)[incMod3(m_index)]; +} + +#endif diff --git a/extern/solid/src/convex/DT_GJK.h b/extern/solid/src/convex/DT_GJK.h new file mode 100755 index 00000000000..d8f44acf85e --- /dev/null +++ b/extern/solid/src/convex/DT_GJK.h @@ -0,0 +1,438 @@ +/* + * 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. + */ + +#ifndef DT_GJK_H +#define DT_GJK_H + +//#define USE_BACKUP_PROCEDURE +#define JOHNSON_ROBUST +#define FAST_CLOSEST + +#include "MT_Point3.h" +#include "MT_Vector3.h" +#include "GEN_MinMax.h" +#include "DT_Accuracy.h" + + +class DT_GJK { +private: + typedef unsigned int T_Bits; + inline static bool subseteq(T_Bits a, T_Bits b) { return (a & b) == a; } + inline static bool contains(T_Bits a, T_Bits b) { return (a & b) != 0x0; } + +public: + DT_GJK() : + m_bits(0x0), + m_all_bits(0x0) + {} + + bool emptySimplex() const { return m_bits == 0x0; } + bool fullSimplex() const { return m_bits == 0xf; } + + void reset() + { + m_bits = 0x0; + m_all_bits = 0x0; + } + + bool inSimplex(const MT_Vector3& w) + { + int i; + T_Bits bit; + for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) + { + if (contains(m_all_bits, bit) && w == m_y[i]) + { + return true; + } + } + return false; + } + + void addVertex(const MT_Vector3& w) + { + assert(!fullSimplex()); + m_last = 0; + m_last_bit = 0x1; + while (contains(m_bits, m_last_bit)) + { + ++m_last; + m_last_bit <<= 1; + } + m_y[m_last] = w; + m_ylen2[m_last] = w.length2(); + m_all_bits = m_bits | m_last_bit; + + update_cache(); + compute_det(); + } + + void addVertex(const MT_Vector3& w, const MT_Point3& p, const MT_Point3& q) + { + addVertex(w); + m_p[m_last] = p; + m_q[m_last] = q; + } + + int getSimplex(MT_Point3 *pBuf, MT_Point3 *qBuf, MT_Vector3 *yBuf) const + { + int num_verts = 0; + int i; + T_Bits bit; + for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) + { + if (contains(m_bits, bit)) + { + pBuf[num_verts] = m_p[i]; + qBuf[num_verts] = m_q[i]; + yBuf[num_verts] = m_y[i]; + +#ifdef DEBUG + std::cout << "Point " << i << " = " << m_y[i] << std::endl; +#endif + + ++num_verts; + } + } + return num_verts; + } + + void compute_points(MT_Point3& p1, MT_Point3& p2) + { + MT_Scalar sum = MT_Scalar(0.0); + p1.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); + p2.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); + int i; + T_Bits bit; + for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) + { + if (contains(m_bits, bit)) + { + sum += m_det[m_bits][i]; + p1 += m_p[i] * m_det[m_bits][i]; + p2 += m_q[i] * m_det[m_bits][i]; + } + } + + assert(sum > MT_Scalar(0.0)); + MT_Scalar s = MT_Scalar(1.0) / sum; + p1 *= s; + p2 *= s; + } + + bool closest(MT_Vector3& v) + { +#ifdef FAST_CLOSEST + T_Bits s; + for (s = m_bits; s != 0x0; --s) + { + if (subseteq(s, m_bits) && valid(s | m_last_bit)) + { + m_bits = s | m_last_bit; + compute_vector(m_bits, v); + return true; + } + } + if (valid(m_last_bit)) + { + m_bits = m_last_bit; + m_maxlen2 = m_ylen2[m_last]; + v = m_y[m_last]; + return true; + } +#else + T_Bits s; + for (s = m_all_bits; s != 0x0; --s) + { + if (subseteq(s, m_all_bits) && valid(s)) + { + m_bits = s; + compute_vector(m_bits, v); + return true; + } + } +#endif + + // Original GJK calls the backup procedure at this point. +#ifdef USE_BACKUP_PROCEDURE + backup_closest(MT_Vector3& v); +#endif + return false; + } + + void backup_closest(MT_Vector3& v) + { + MT_Scalar min_dist2 = MT_INFINITY; + + T_Bits s; + for (s = m_all_bits; s != 0x0; --s) + { + if (subseteq(s, m_all_bits) && proper(s)) + { + MT_Vector3 u; + compute_vector(s, u); + MT_Scalar dist2 = u.length2(); + if (dist2 < min_dist2) + { + min_dist2 = dist2; + m_bits = s; + v = u; + } + } + } + } + + MT_Scalar maxVertex() { return m_maxlen2; } + + +private: + void update_cache(); + void compute_det(); + + bool valid(T_Bits s) + { + int i; + T_Bits bit; + for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) + { + if (contains(m_all_bits, bit)) + { + if (contains(s, bit)) + { + if (m_det[s][i] <= MT_Scalar(0.0)) + { + return false; + } + } + else if (m_det[s | bit][i] > MT_Scalar(0.0)) + { + return false; + } + } + } + return true; + } + + bool proper(T_Bits s) + { + int i; + T_Bits bit; + for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) + { + if (contains(s, bit) && m_det[s][i] <= MT_Scalar(0.0)) + { + return false; + } + } + return true; + } + + void compute_vector(T_Bits s, MT_Vector3& v) + { + m_maxlen2 = MT_Scalar(0.0); + MT_Scalar sum = MT_Scalar(0.0); + v .setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); + + int i; + T_Bits bit; + for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) + { + if (contains(s, bit)) + { + sum += m_det[s][i]; + GEN_set_max(m_maxlen2, m_ylen2[i]); + v += m_y[i] * m_det[s][i]; + } + } + + assert(sum > MT_Scalar(0.0)); + + v /= sum; + } + +private: + MT_Scalar m_det[16][4]; // cached sub-determinants + MT_Vector3 m_edge[4][4]; + +#ifdef JOHNSON_ROBUST + MT_Scalar m_norm[4][4]; +#endif + + MT_Point3 m_p[4]; // support points of object A in local coordinates + MT_Point3 m_q[4]; // support points of object B in local coordinates + MT_Vector3 m_y[4]; // support points of A - B in world coordinates + MT_Scalar m_ylen2[4]; // Squared lengths support points y + + MT_Scalar m_maxlen2; // Maximum squared length to a vertex of the current + // simplex + T_Bits m_bits; // identifies current simplex + T_Bits m_last; // identifies last found support point + T_Bits m_last_bit; // m_last_bit == 0x1 << last + T_Bits m_all_bits; // m_all_bits == m_bits | m_last_bit +}; + + + + +inline void DT_GJK::update_cache() +{ + int i; + T_Bits bit; + for (i = 0, bit = 0x1; i < 4; ++i, bit <<= 1) + { + if (contains(m_bits, bit)) + { + m_edge[i][m_last] = m_y[i] - m_y[m_last]; + m_edge[m_last][i] = -m_edge[i][m_last]; + +#ifdef JOHNSON_ROBUST + m_norm[i][m_last] = m_norm[m_last][i] = m_edge[i][m_last].length2(); +#endif + + } + } +} + +#ifdef JOHNSON_ROBUST + +inline void DT_GJK::compute_det() +{ + m_det[m_last_bit][m_last] = 1; + + int i; + T_Bits si; + for (i = 0, si = 0x1; i < 4; ++i, si <<= 1) + { + if (contains(m_bits, si)) + { + T_Bits s2 = si | m_last_bit; + m_det[s2][i] = m_edge[m_last][i].dot(m_y[m_last]); + m_det[s2][m_last] = m_edge[i][m_last].dot(m_y[i]); + + int j; + T_Bits sj; + for (j = 0, sj = 0x1; j < i; ++j, sj <<= 1) + { + if (contains(m_bits, sj)) + { + int k; + T_Bits s3 = sj | s2; + + k = m_norm[i][j] < m_norm[m_last][j] ? i : m_last; + m_det[s3][j] = m_det[s2][i] * m_edge[k][j].dot(m_y[i]) + + m_det[s2][m_last] * m_edge[k][j].dot(m_y[m_last]); + k = m_norm[j][i] < m_norm[m_last][i] ? j : m_last; + m_det[s3][i] = m_det[sj|m_last_bit][j] * m_edge[k][i].dot(m_y[j]) + + m_det[sj|m_last_bit][m_last] * m_edge[k][i].dot(m_y[m_last]); + k = m_norm[i][m_last] < m_norm[j][m_last] ? i : j; + m_det[s3][m_last] = m_det[sj|si][j] * m_edge[k][m_last].dot(m_y[j]) + + m_det[sj|si][i] * m_edge[k][m_last].dot(m_y[i]); + } + } + } + } + + if (m_all_bits == 0xf) + { + int k; + + k = m_norm[1][0] < m_norm[2][0] ? (m_norm[1][0] < m_norm[3][0] ? 1 : 3) : (m_norm[2][0] < m_norm[3][0] ? 2 : 3); + + m_det[0xf][0] = m_det[0xe][1] * m_edge[k][0].dot(m_y[1]) + + m_det[0xe][2] * m_edge[k][0].dot(m_y[2]) + + m_det[0xe][3] * m_edge[k][0].dot(m_y[3]); + + k = m_norm[0][1] < m_norm[2][1] ? (m_norm[0][1] < m_norm[3][1] ? 0 : 3) : (m_norm[2][1] < m_norm[3][1] ? 2 : 3); + + m_det[0xf][1] = m_det[0xd][0] * m_edge[k][1].dot(m_y[0]) + + m_det[0xd][2] * m_edge[k][1].dot(m_y[2]) + + m_det[0xd][3] * m_edge[k][1].dot(m_y[3]); + + k = m_norm[0][2] < m_norm[1][2] ? (m_norm[0][2] < m_norm[3][2] ? 0 : 3) : (m_norm[1][2] < m_norm[3][2] ? 1 : 3); + + m_det[0xf][2] = m_det[0xb][0] * m_edge[k][2].dot(m_y[0]) + + m_det[0xb][1] * m_edge[k][2].dot(m_y[1]) + + m_det[0xb][3] * m_edge[k][2].dot(m_y[3]); + + k = m_norm[0][3] < m_norm[1][3] ? (m_norm[0][3] < m_norm[2][3] ? 0 : 2) : (m_norm[1][3] < m_norm[2][3] ? 1 : 2); + + m_det[0xf][3] = m_det[0x7][0] * m_edge[k][3].dot(m_y[0]) + + m_det[0x7][1] * m_edge[k][3].dot(m_y[1]) + + m_det[0x7][2] * m_edge[k][3].dot(m_y[2]); + } +} + +#else + +inline void DT_GJK::compute_det() +{ + m_det[m_last_bit][m_last] = 1; + + int i; + T_Bits si; + for (i = 0, si = 0x1; i < 4; ++i, si <<= 1) + { + if (contains(m_bits, si)) + { + T_Bits s2 = si | m_last_bit; + m_det[s2][i] = m_edge[m_last][i].dot(m_y[m_last]); + m_det[s2][m_last] = m_edge[i][m_last].dot(m_y[i]); + + int j; + T_Bits sj; + for (j = 0, sj = 0x1; j < i; ++j, sj <<= 1) + { + if (contains(m_bits, sj)) + { + T_Bits s3 = sj | s2; + m_det[s3][j] = m_det[s2][i] * m_edge[i][j].dot(m_y[i]) + + m_det[s2][m_last] * m_edge[i][j].dot(m_y[m_last]); + m_det[s3][i] = m_det[sj|m_last_bit][j] * m_edge[j][i].dot(m_y[j]) + + m_det[sj|m_last_bit][m_last] * m_edge[j][i].dot(m_y[m_last]); + m_det[s3][m_last] = m_det[sj|si][j] * m_edge[j][m_last].dot(m_y[j]) + + m_det[sj|si][i] * m_edge[j][m_last].dot(m_y[i]); + } + } + } + } + + if (m_all_bits == 0xf) + { + m_det[0xf][0] = m_det[0xe][1] * m_edge[1][0].dot(m_y[1]) + + m_det[0xe][2] * m_edge[1][0].dot(m_y[2]) + + m_det[0xe][3] * m_edge[1][0].dot(m_y[3]); + m_det[0xf][1] = m_det[0xd][0] * m_edge[0][1].dot(m_y[0]) + + m_det[0xd][2] * m_edge[0][1].dot(m_y[2]) + + m_det[0xd][3] * m_edge[0][1].dot(m_y[3]); + m_det[0xf][2] = m_det[0xb][0] * m_edge[0][2].dot(m_y[0]) + + m_det[0xb][1] * m_edge[0][2].dot(m_y[1]) + + m_det[0xb][3] * m_edge[0][2].dot(m_y[3]); + m_det[0xf][3] = m_det[0x7][0] * m_edge[0][3].dot(m_y[0]) + + m_det[0x7][1] * m_edge[0][3].dot(m_y[1]) + + m_det[0x7][2] * m_edge[0][3].dot(m_y[2]); + } +} + +#endif + +#endif diff --git a/extern/solid/src/convex/DT_Hull.h b/extern/solid/src/convex/DT_Hull.h new file mode 100755 index 00000000000..a5bf56ae59d --- /dev/null +++ b/extern/solid/src/convex/DT_Hull.h @@ -0,0 +1,53 @@ +/* + * 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. + */ + +#ifndef DT_HULL_H +#define DT_HULL_H + +#include "DT_Convex.h" + +class DT_Hull : public DT_Convex { +public: + DT_Hull(const DT_Convex& lchild, const DT_Convex& rchild) : + m_lchild(lchild), + m_rchild(rchild) + {} + + virtual MT_Scalar supportH(const MT_Vector3& v) const + { + return GEN_max(m_lchild.supportH(v), m_rchild.supportH(v)); + } + + virtual MT_Point3 support(const MT_Vector3& v) const + { + MT_Point3 lpnt = m_lchild.support(v); + MT_Point3 rpnt = m_rchild.support(v); + return v.dot(lpnt) > v.dot(rpnt) ? lpnt : rpnt; + } + +private: + const DT_Convex& m_lchild; + const DT_Convex& m_rchild; +}; + +#endif diff --git a/extern/solid/src/convex/DT_IndexArray.h b/extern/solid/src/convex/DT_IndexArray.h new file mode 100755 index 00000000000..95551fa8483 --- /dev/null +++ b/extern/solid/src/convex/DT_IndexArray.h @@ -0,0 +1,33 @@ +/* + * 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. + */ + +#ifndef DT_INDEXARRAY_H +#define DT_INDEXARRAY_H + +#include "SOLID_types.h" +#include "DT_Array.h" + +typedef DT_Array<DT_Index, DT_Count> DT_IndexArray; + +#endif + diff --git a/extern/solid/src/convex/DT_LineSegment.cpp b/extern/solid/src/convex/DT_LineSegment.cpp new file mode 100755 index 00000000000..6c7ccf6b9b7 --- /dev/null +++ b/extern/solid/src/convex/DT_LineSegment.cpp @@ -0,0 +1,36 @@ +/* + * 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_LineSegment.h" + +MT_Scalar DT_LineSegment::supportH(const MT_Vector3& v) const +{ + return GEN_max(v.dot(m_source), v.dot(m_target)); +} + +MT_Point3 DT_LineSegment::support(const MT_Vector3& v) const +{ + return v.dot(m_source) > v.dot(m_target) ? m_source : m_target; +} + + diff --git a/extern/solid/src/convex/DT_LineSegment.h b/extern/solid/src/convex/DT_LineSegment.h new file mode 100755 index 00000000000..979ff8a18a9 --- /dev/null +++ b/extern/solid/src/convex/DT_LineSegment.h @@ -0,0 +1,52 @@ +/* + * 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. + */ + +#ifndef DT_LINESEGMENT_H +#define DT_LINESEGMENT_H + +#include "DT_Convex.h" + +class DT_LineSegment : public DT_Convex { +public: + DT_LineSegment(const MT_Point3& source, const MT_Point3& target) : + m_source(source), + m_target(target) {} + + virtual MT_Scalar supportH(const MT_Vector3& v) const; + virtual MT_Point3 support(const MT_Vector3& v) const; + + const MT_Point3& getSource() const { return m_source; } + const MT_Point3& getTarget() const { return m_target; } + +private: + MT_Point3 m_source; + MT_Point3 m_target; +}; + +#endif + + + + + + diff --git a/extern/solid/src/convex/DT_Minkowski.h b/extern/solid/src/convex/DT_Minkowski.h new file mode 100755 index 00000000000..e90fed6a8e0 --- /dev/null +++ b/extern/solid/src/convex/DT_Minkowski.h @@ -0,0 +1,51 @@ +/* + * 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. + */ + +#ifndef DT_MINKOWSKI_H +#define DT_MINKOWSKI_H + +#include "DT_Convex.h" + +class DT_Minkowski : public DT_Convex { +public: + DT_Minkowski(const DT_Convex& lchild, const DT_Convex& rchild) + : m_lchild(lchild), + m_rchild(rchild) + {} + + virtual MT_Scalar supportH(const MT_Vector3& v) const + { + return m_lchild.supportH(v) + m_rchild.supportH(v); + } + + virtual MT_Point3 support(const MT_Vector3& v) const + { + return m_lchild.support(v) + (MT_Vector3)m_rchild.support(v); + } + +private: + const DT_Convex& m_lchild; + const DT_Convex& m_rchild; +}; + +#endif diff --git a/extern/solid/src/convex/DT_PenDepth.cpp b/extern/solid/src/convex/DT_PenDepth.cpp new file mode 100755 index 00000000000..e1c5c9a3949 --- /dev/null +++ b/extern/solid/src/convex/DT_PenDepth.cpp @@ -0,0 +1,376 @@ +/* + * 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_PenDepth.h" + +#include "MT_Vector3.h" +#include "MT_Point3.h" +#include "MT_Quaternion.h" +#include "DT_Convex.h" +#include "DT_GJK.h" +#include "DT_Facet.h" + +//#define DEBUG + +const int MaxSupportPoints = 1000; +const int MaxFacets = 2000; + +static MT_Point3 pBuf[MaxSupportPoints]; +static MT_Point3 qBuf[MaxSupportPoints]; +static MT_Vector3 yBuf[MaxSupportPoints]; + +static DT_Facet facetBuf[MaxFacets]; +static int freeFacet = 0; +static DT_Facet *facetHeap[MaxFacets]; +static int num_facets; + +class DT_FacetComp { +public: + + bool operator()(const DT_Facet *face1, const DT_Facet *face2) + { + return face1->getDist2() > face2->getDist2(); + } + +} facetComp; + +inline DT_Facet *addFacet(int i0, int i1, int i2, + MT_Scalar lower2, MT_Scalar upper2) +{ + assert(i0 != i1 && i0 != i2 && i1 != i2); + if (freeFacet < MaxFacets) + { + DT_Facet *facet = new(&facetBuf[freeFacet++]) DT_Facet(i0, i1, i2); +#ifdef DEBUG + std::cout << "Facet " << i0 << ' ' << i1 << ' ' << i2; +#endif + if (facet->computeClosest(yBuf)) + { + if (facet->isClosestInternal() && + lower2 <= facet->getDist2() && facet->getDist2() <= upper2) + { + facetHeap[num_facets++] = facet; + std::push_heap(&facetHeap[0], &facetHeap[num_facets], facetComp); +#ifdef DEBUG + std::cout << " accepted" << std::endl; +#endif + } + else + { +#ifdef DEBUG + std::cout << " rejected, "; + if (!facet->isClosestInternal()) + { + std::cout << "closest point not internal"; + } + else if (lower2 > facet->getDist2()) + { + std::cout << "facet is closer than orignal facet"; + } + else + { + std::cout << "facet is further than upper bound"; + } + std::cout << std::endl; +#endif + } + + return facet; + } + } + + return 0; +} + +inline bool originInTetrahedron(const MT_Vector3& p1, const MT_Vector3& p2, + const MT_Vector3& p3, const MT_Vector3& p4) +{ + MT_Vector3 normal1 = (p2 - p1).cross(p3 - p1); + MT_Vector3 normal2 = (p3 - p2).cross(p4 - p2); + MT_Vector3 normal3 = (p4 - p3).cross(p1 - p3); + MT_Vector3 normal4 = (p1 - p4).cross(p2 - p4); + + return + normal1.dot(p1) > MT_Scalar(0.0) != normal1.dot(p4) > MT_Scalar(0.0) && + normal2.dot(p2) > MT_Scalar(0.0) != normal2.dot(p1) > MT_Scalar(0.0) && + normal3.dot(p3) > MT_Scalar(0.0) != normal3.dot(p2) > MT_Scalar(0.0) && + normal4.dot(p4) > MT_Scalar(0.0) != normal4.dot(p3) > MT_Scalar(0.0); +} + + +bool penDepth(const DT_GJK& gjk, const DT_Convex& a, const DT_Convex& b, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb) +{ + + int num_verts = gjk.getSimplex(pBuf, qBuf, yBuf); + + switch (num_verts) + { + case 1: + // Touching contact. Yes, we have a collision, + // but no penetration. + return false; + case 2: + { + // We have a line segment inside the Minkowski sum containing the + // origin. Blow it up by adding three additional support points. + + MT_Vector3 dir = (yBuf[1] - yBuf[0]).normalized(); + int axis = dir.furthestAxis(); + + static MT_Scalar sin_60 = MT_sqrt(MT_Scalar(3.0)) * MT_Scalar(0.5); + + MT_Quaternion rot(dir[0] * sin_60, dir[1] * sin_60, dir[2] * sin_60, MT_Scalar(0.5)); + MT_Matrix3x3 rot_mat(rot); + + MT_Vector3 aux1 = dir.cross(MT_Vector3(axis == 0, axis == 1, axis == 2)); + MT_Vector3 aux2 = rot_mat * aux1; + MT_Vector3 aux3 = rot_mat * aux2; + + pBuf[2] = a.support(aux1); + qBuf[2] = b.support(-aux1); + yBuf[2] = pBuf[2] - qBuf[2]; + + pBuf[3] = a.support(aux2); + qBuf[3] = b.support(-aux2); + yBuf[3] = pBuf[3] - qBuf[3]; + + pBuf[4] = a.support(aux3); + qBuf[4] = b.support(-aux3); + yBuf[4] = pBuf[4] - qBuf[4]; + + if (originInTetrahedron(yBuf[0], yBuf[2], yBuf[3], yBuf[4])) + { + pBuf[1] = pBuf[4]; + qBuf[1] = qBuf[4]; + yBuf[1] = yBuf[4]; + } + else if (originInTetrahedron(yBuf[1], yBuf[2], yBuf[3], yBuf[4])) + { + pBuf[0] = pBuf[4]; + qBuf[0] = qBuf[4]; + yBuf[0] = yBuf[4]; + } + else + { + // Origin not in initial polytope + return false; + } + + num_verts = 4; + + break; + } + case 3: + { + // We have a triangle inside the Minkowski sum containing + // the origin. First blow it up. + + MT_Vector3 v1 = yBuf[1] - yBuf[0]; + MT_Vector3 v2 = yBuf[2] - yBuf[0]; + MT_Vector3 vv = v1.cross(v2); + + pBuf[3] = a.support(vv); + qBuf[3] = b.support(-vv); + yBuf[3] = pBuf[3] - qBuf[3]; + pBuf[4] = a.support(-vv); + qBuf[4] = b.support(vv); + yBuf[4] = pBuf[4] - qBuf[4]; + + + if (originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[4])) + { + pBuf[3] = pBuf[4]; + qBuf[3] = qBuf[4]; + yBuf[3] = yBuf[4]; + } + else if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3])) + { + // Origin not in initial polytope + return false; + } + + num_verts = 4; + + break; + } + } + + // We have a tetrahedron inside the Minkowski sum containing + // the origin (if GJK did it's job right ;-) + + + if (!originInTetrahedron(yBuf[0], yBuf[1], yBuf[2], yBuf[3])) + { + // assert(false); + return false; + } + + num_facets = 0; + freeFacet = 0; + + DT_Facet *f0 = addFacet(0, 1, 2, MT_Scalar(0.0), MT_INFINITY); + DT_Facet *f1 = addFacet(0, 3, 1, MT_Scalar(0.0), MT_INFINITY); + DT_Facet *f2 = addFacet(0, 2, 3, MT_Scalar(0.0), MT_INFINITY); + DT_Facet *f3 = addFacet(1, 3, 2, MT_Scalar(0.0), MT_INFINITY); + + if (!f0 || f0->getDist2() == MT_Scalar(0.0) || + !f1 || f1->getDist2() == MT_Scalar(0.0) || + !f2 || f2->getDist2() == MT_Scalar(0.0) || + !f3 || f3->getDist2() == MT_Scalar(0.0)) + { + return false; + } + + f0->link(0, f1, 2); + f0->link(1, f3, 2); + f0->link(2, f2, 0); + f1->link(0, f2, 2); + f1->link(1, f3, 0); + f2->link(1, f3, 1); + + if (num_facets == 0) + { + return false; + } + + // at least one facet on the heap. + + DT_EdgeBuffer edgeBuffer(20); + + DT_Facet *facet = 0; + + MT_Scalar upper_bound2 = MT_INFINITY; + + do { + facet = facetHeap[0]; + std::pop_heap(&facetHeap[0], &facetHeap[num_facets], facetComp); + --num_facets; + + if (!facet->isObsolete()) + { + assert(facet->getDist2() > MT_Scalar(0.0)); + + if (num_verts == MaxSupportPoints) + { +#ifdef DEBUG + std::cout << "Ouch, no convergence!!!" << std::endl; +#endif + assert(false); + break; + } + + pBuf[num_verts] = a.support(facet->getClosest()); + qBuf[num_verts] = b.support(-facet->getClosest()); + yBuf[num_verts] = pBuf[num_verts] - qBuf[num_verts]; + + int index = num_verts++; + MT_Scalar far_dist2 = yBuf[index].dot(facet->getClosest()); + + // Make sure the support mapping is OK. + assert(far_dist2 > MT_Scalar(0.0)); + + GEN_set_min(upper_bound2, far_dist2 * far_dist2 / facet->getDist2()); + + if (upper_bound2 <= DT_Accuracy::depth_tolerance * facet->getDist2() +#define CHECK_NEW_SUPPORT +#ifdef CHECK_NEW_SUPPORT + || yBuf[index] == yBuf[(*facet)[0]] + || yBuf[index] == yBuf[(*facet)[1]] + || yBuf[index] == yBuf[(*facet)[2]] +#endif + ) + { + break; + } + + // Compute the silhouette cast by the new vertex + // Note that the new vertex is on the positive side + // of the current facet, so the current facet is will + // not be in the convex hull. Start local search + // from this facet. + + facet->silhouette(yBuf[index], edgeBuffer); + + if (edgeBuffer.empty()) + { + return false; + } + + DT_EdgeBuffer::const_iterator it = edgeBuffer.begin(); + DT_Facet *firstFacet = + addFacet((*it).getTarget(), (*it).getSource(), + index, facet->getDist2(), upper_bound2); + + if (!firstFacet) + { + break; + } + + firstFacet->link(0, (*it).getFacet(), (*it).getIndex()); + DT_Facet *lastFacet = firstFacet; + + ++it; + for (; it != edgeBuffer.end(); ++it) + { + DT_Facet *newFacet = + addFacet((*it).getTarget(), (*it).getSource(), + index, facet->getDist2(), upper_bound2); + + if (!newFacet) + { + break; + } + + if (!newFacet->link(0, (*it).getFacet(), (*it).getIndex())) + { + break; + } + + if (!newFacet->link(2, lastFacet, 1)) + { + break; + } + + lastFacet = newFacet; + } + if (it != edgeBuffer.end()) + { + break; + } + + firstFacet->link(2, lastFacet, 1); + } + } + while (num_facets > 0 && facetHeap[0]->getDist2() <= upper_bound2); + +#ifdef DEBUG + std::cout << "#facets left = " << num_facets << std::endl; +#endif + + v = facet->getClosest(); + pa = facet->getClosestPoint(pBuf); + pb = facet->getClosestPoint(qBuf); + return true; +} + diff --git a/extern/solid/src/convex/DT_PenDepth.h b/extern/solid/src/convex/DT_PenDepth.h new file mode 100755 index 00000000000..97b3c6c0e0e --- /dev/null +++ b/extern/solid/src/convex/DT_PenDepth.h @@ -0,0 +1,36 @@ +/* + * 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. + */ + +#ifndef DT_PENDEPTH_H +#define DT_PENDEPTH_H + +#include "MT_Vector3.h" +#include "MT_Point3.h" + +class DT_GJK; +class DT_Convex; + +bool penDepth(const DT_GJK& gjk, const DT_Convex& a, const DT_Convex& b, + MT_Vector3& v, MT_Point3& pa, MT_Point3& pb); + +#endif diff --git a/extern/solid/src/convex/DT_Point.cpp b/extern/solid/src/convex/DT_Point.cpp new file mode 100755 index 00000000000..770fe7775b7 --- /dev/null +++ b/extern/solid/src/convex/DT_Point.cpp @@ -0,0 +1,36 @@ +/* + * 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_Point.h" + +MT_Scalar DT_Point::supportH(const MT_Vector3& v) const +{ + return v.dot(m_point); +} + +MT_Point3 DT_Point::support(const MT_Vector3& v) const +{ + return m_point; +} + + diff --git a/extern/solid/src/convex/DT_Point.h b/extern/solid/src/convex/DT_Point.h new file mode 100755 index 00000000000..b35d158ee53 --- /dev/null +++ b/extern/solid/src/convex/DT_Point.h @@ -0,0 +1,46 @@ +/* + * 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. + */ + +#ifndef DT_POINT_H +#define DT_POINT_H + +#include "DT_Convex.h" + +class DT_Point : public DT_Convex { +public: + DT_Point(const MT_Point3& point) : m_point(point) {} + + virtual MT_Scalar supportH(const MT_Vector3& v) const; + virtual MT_Point3 support(const MT_Vector3& v) const; + +private: + MT_Point3 m_point; +}; + +#endif + + + + + + diff --git a/extern/solid/src/convex/DT_Polyhedron.cpp b/extern/solid/src/convex/DT_Polyhedron.cpp new file mode 100755 index 00000000000..f48ac6e4b6d --- /dev/null +++ b/extern/solid/src/convex/DT_Polyhedron.cpp @@ -0,0 +1,415 @@ +/* + * 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_Polyhedron.h" + +#ifdef QHULL + +extern "C" { +#include <qhull/qhull_a.h> +} + +#include <vector> +#include <new> + +typedef std::vector<MT_Point3> T_VertexBuf; +typedef std::vector<DT_Index> T_IndexBuf; +typedef std::vector<T_IndexBuf> T_MultiIndexBuf; + +static char options[] = "qhull Qts i Tv"; + +#define DK_HIERARCHY + +T_IndexBuf *adjacency_graph(DT_Count count, const MT_Point3 *verts, const char *flags) +{ + int curlong, totlong, exitcode; + + facetT *facet; + vertexT *vertex; + vertexT **vertexp; + + std::vector<MT::Tuple3<coordT> > array; + T_IndexBuf index; + DT_Index i; + for (i = 0; i != count; ++i) + { + if (flags == 0 || flags[i]) + { + array.push_back(MT::Tuple3<coordT>(verts[i])); + index.push_back(i); + } + } + + qh_init_A(stdin, stdout, stderr, 0, NULL); + if ((exitcode = setjmp(qh errexit))) + { + exit(exitcode); + } + qh_initflags(options); + qh_init_B(array[0], array.size(), 3, False); + qh_qhull(); + qh_check_output(); + + T_IndexBuf *indexBuf = new T_IndexBuf[count]; + FORALLfacets + { + setT *vertices = qh_facet3vertex(facet); + + T_IndexBuf facetIndices; + + FOREACHvertex_(vertices) + { + facetIndices.push_back(index[qh_pointid(vertex->point)]); + } + int i, j; + for (i = 0, j = facetIndices.size()-1; i < (int)facetIndices.size(); j = i++) + { + indexBuf[facetIndices[j]].push_back(facetIndices[i]); + } + } + + + qh NOerrexit = True; + qh_freeqhull(!qh_ALL); + qh_memfreeshort(&curlong, &totlong); + + return indexBuf; +} + +T_IndexBuf *simplex_adjacency_graph(DT_Count count, const char *flags) +{ + T_IndexBuf *indexBuf = new T_IndexBuf[count]; + + DT_Index index[4]; + + DT_Index k = 0; + DT_Index i; + for (i = 0; i != count; ++i) + { + if (flags == 0 || flags[i]) + { + index[k++] = i; + } + } + + assert(k <= 4); + + for (i = 0; i != k; ++i) + { + DT_Index j; + for (j = 0; j != k; ++j) + { + if (i != j) + { + indexBuf[index[i]].push_back(index[j]); + } + } + } + + return indexBuf; +} + +#ifdef DK_HIERARCHY + +void prune(DT_Count count, T_MultiIndexBuf *cobound) +{ + DT_Index i; + for (i = 0; i != count; ++i) + { + assert(cobound[i].size()); + + DT_Index j; + for (j = 0; j != cobound[i].size() - 1; ++j) + { + T_IndexBuf::iterator it = cobound[i][j].begin(); + while (it != cobound[i][j].end()) + { + T_IndexBuf::iterator jt = + std::find(cobound[i][j+1].begin(), cobound[i][j+1].end(), *it); + + if (jt != cobound[i][j+1].end()) + { + std::swap(*it, cobound[i][j].back()); + cobound[i][j].pop_back(); + } + else + { + ++it; + } + } + } + } +} + +#endif + +DT_Polyhedron::DT_Polyhedron(const DT_VertexBase *base, DT_Count count, const DT_Index *indices) +{ + assert(count); + + std::vector<MT_Point3> vertexBuf; + DT_Index i; + for (i = 0; i != count; ++i) + { + vertexBuf.push_back((*base)[indices[i]]); + } + + T_IndexBuf *indexBuf = count > 4 ? adjacency_graph(count, &vertexBuf[0], 0) : simplex_adjacency_graph(count, 0); + + std::vector<MT_Point3> pointBuf; + + for (i = 0; i != count; ++i) + { + if (!indexBuf[i].empty()) + { + pointBuf.push_back(vertexBuf[i]); + } + } + + delete [] indexBuf; + + m_count = pointBuf.size(); + m_verts = new MT_Point3[m_count]; + std::copy(pointBuf.begin(), pointBuf.end(), &m_verts[0]); + + T_MultiIndexBuf *cobound = new T_MultiIndexBuf[m_count]; + char *flags = new char[m_count]; + std::fill(&flags[0], &flags[m_count], 1); + + DT_Count num_layers = 0; + DT_Count layer_count = m_count; + while (layer_count > 4) + { + T_IndexBuf *indexBuf = adjacency_graph(m_count, m_verts, flags); + + DT_Index i; + for (i = 0; i != m_count; ++i) + { + if (flags[i]) + { + assert(!indexBuf[i].empty()); + cobound[i].push_back(indexBuf[i]); + } + } + + ++num_layers; + + delete [] indexBuf; + + std::fill(&flags[0], &flags[m_count], 0); + + for (i = 0; i != m_count; ++i) + { + if (cobound[i].size() == num_layers) + { + T_IndexBuf& curr_cobound = cobound[i].back(); + if (!flags[i] && curr_cobound.size() <= 8) + { + DT_Index j; + for (j = 0; j != curr_cobound.size(); ++j) + { + flags[curr_cobound[j]] = 1; + } + } + } + } + + layer_count = 0; + + for (i = 0; i != m_count; ++i) + { + if (flags[i]) + { + ++layer_count; + } + } + } + + indexBuf = simplex_adjacency_graph(m_count, flags); + + for (i = 0; i != m_count; ++i) + { + if (flags[i]) + { + assert(!indexBuf[i].empty()); + cobound[i].push_back(indexBuf[i]); + } + } + + ++num_layers; + + delete [] indexBuf; + delete [] flags; + + + +#ifdef DK_HIERARCHY + prune(m_count, cobound); +#endif + + m_cobound = new T_MultiIndexArray[m_count]; + + for (i = 0; i != m_count; ++i) + { + new (&m_cobound[i]) T_MultiIndexArray(cobound[i].size()); + + DT_Index j; + for (j = 0; j != cobound[i].size(); ++j) + { + new (&m_cobound[i][j]) DT_IndexArray(cobound[i][j].size(), &cobound[i][j][0]); + } + } + + delete [] cobound; + + m_start_vertex = 0; + while (m_cobound[m_start_vertex].size() != num_layers) + { + ++m_start_vertex; + assert(m_start_vertex < m_count); + } + + m_curr_vertex = m_start_vertex; +} + + +DT_Polyhedron::~DT_Polyhedron() +{ + delete [] m_verts; + delete [] m_cobound; +} + +#ifdef DK_HIERARCHY + +MT_Scalar DT_Polyhedron::supportH(const MT_Vector3& v) const +{ + m_curr_vertex = m_start_vertex; + MT_Scalar d = (*this)[m_curr_vertex].dot(v); + MT_Scalar h = d; + int curr_layer; + for (curr_layer = m_cobound[m_start_vertex].size(); curr_layer != 0; --curr_layer) + { + const DT_IndexArray& curr_cobound = m_cobound[m_curr_vertex][curr_layer-1]; + DT_Index i; + for (i = 0; i != curr_cobound.size(); ++i) + { + d = (*this)[curr_cobound[i]].dot(v); + if (d > h) + { + m_curr_vertex = curr_cobound[i]; + h = d; + } + } + } + + return h; +} + +MT_Point3 DT_Polyhedron::support(const MT_Vector3& v) const +{ + m_curr_vertex = m_start_vertex; + MT_Scalar d = (*this)[m_curr_vertex].dot(v); + MT_Scalar h = d; + int curr_layer; + for (curr_layer = m_cobound[m_start_vertex].size(); curr_layer != 0; --curr_layer) + { + const DT_IndexArray& curr_cobound = m_cobound[m_curr_vertex][curr_layer-1]; + DT_Index i; + for (i = 0; i != curr_cobound.size(); ++i) + { + d = (*this)[curr_cobound[i]].dot(v); + if (d > h) + { + m_curr_vertex = curr_cobound[i]; + h = d; + } + } + } + + return (*this)[m_curr_vertex]; +} + +#else + +MT_Scalar DT_Polyhedron::supportH(const MT_Vector3& v) const +{ + int last_vertex = -1; + MT_Scalar d = (*this)[m_curr_vertex].dot(v); + MT_Scalar h = d; + + for (;;) + { + DT_IndexArray& curr_cobound = m_cobound[m_curr_vertex][0]; + int i = 0, n = curr_cobound.size(); + while (i != n && + (curr_cobound[i] == last_vertex || + (d = (*this)[curr_cobound[i]].dot(v)) - h <= MT_abs(h) * MT_EPSILON)) + { + ++i; + } + + if (i == n) + { + break; + } + + last_vertex = m_curr_vertex; + m_curr_vertex = curr_cobound[i]; + h = d; + } + return h; +} + +MT_Point3 DT_Polyhedron::support(const MT_Vector3& v) const +{ + int last_vertex = -1; + MT_Scalar d = (*this)[m_curr_vertex].dot(v); + MT_Scalar h = d; + + for (;;) + { + DT_IndexArray& curr_cobound = m_cobound[m_curr_vertex][0]; + int i = 0, n = curr_cobound.size(); + while (i != n && + (curr_cobound[i] == last_vertex || + (d = (*this)[curr_cobound[i]].dot(v)) - h <= MT_abs(h) * MT_EPSILON)) + { + ++i; + } + + if (i == n) + { + break; + } + + last_vertex = m_curr_vertex; + m_curr_vertex = curr_cobound[i]; + h = d; + } + return (*this)[m_curr_vertex]; +} + +#endif + +#endif + diff --git a/extern/solid/src/convex/DT_Polyhedron.h b/extern/solid/src/convex/DT_Polyhedron.h new file mode 100755 index 00000000000..58c991bd152 --- /dev/null +++ b/extern/solid/src/convex/DT_Polyhedron.h @@ -0,0 +1,76 @@ +/* + * 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. + */ + +#ifndef DT_POLYHEDRON_H +#define DT_POLYHEDRON_H + +#ifdef HAVE_CONFIG_H +# include "config.h" +# if HAVE_QHULL_QHULL_A_H +# define QHULL +# endif +#endif + + +#ifdef QHULL + +#include "DT_Convex.h" +#include "DT_IndexArray.h" +#include "DT_VertexBase.h" + +class DT_Polyhedron : public DT_Convex { + typedef DT_Array<DT_IndexArray> T_MultiIndexArray; +public: + DT_Polyhedron() + : m_verts(0), + m_cobound(0) + {} + + DT_Polyhedron(const DT_VertexBase *base, DT_Count count, const DT_Index *indices); + + virtual ~DT_Polyhedron(); + + virtual MT_Scalar supportH(const MT_Vector3& v) const; + virtual MT_Point3 support(const MT_Vector3& v) const; + + const MT_Point3& operator[](int i) const { return m_verts[i]; } + DT_Count numVerts() const { return m_count; } + +private: + DT_Count m_count; + MT_Point3 *m_verts; + T_MultiIndexArray *m_cobound; + DT_Index m_start_vertex; + mutable DT_Index m_curr_vertex; +}; + +#else + +#include "DT_Polytope.h" + +typedef DT_Polytope DT_Polyhedron; + +#endif + +#endif + diff --git a/extern/solid/src/convex/DT_Polytope.cpp b/extern/solid/src/convex/DT_Polytope.cpp new file mode 100755 index 00000000000..e757c3bfdb4 --- /dev/null +++ b/extern/solid/src/convex/DT_Polytope.cpp @@ -0,0 +1,69 @@ +/* + * 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_Polytope.h" + +MT_BBox DT_Polytope::bbox() const +{ + MT_BBox bbox = (*this)[0]; + DT_Index i; + for (i = 1; i < numVerts(); ++i) + { + bbox = bbox.hull((*this)[i]); + } + return bbox; +} + +MT_Scalar DT_Polytope::supportH(const MT_Vector3& v) const +{ + int c = 0; + MT_Scalar h = (*this)[0].dot(v), d; + DT_Index i; + for (i = 1; i < numVerts(); ++i) + { + if ((d = (*this)[i].dot(v)) > h) + { + c = i; + h = d; + } + } + return h; +} + +MT_Point3 DT_Polytope::support(const MT_Vector3& v) const +{ + int c = 0; + MT_Scalar h = (*this)[0].dot(v), d; + DT_Index i; + for (i = 1; i < numVerts(); ++i) + { + if ((d = (*this)[i].dot(v)) > h) + { + c = i; + h = d; + } + } + return (*this)[c]; +} + + diff --git a/extern/solid/src/convex/DT_Polytope.h b/extern/solid/src/convex/DT_Polytope.h new file mode 100755 index 00000000000..c715598defe --- /dev/null +++ b/extern/solid/src/convex/DT_Polytope.h @@ -0,0 +1,51 @@ +/* + * 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. + */ + +#ifndef DT_POLYTOPE_H +#define DT_POLYTOPE_H + +#include "DT_Convex.h" +#include "DT_IndexArray.h" +#include "DT_VertexBase.h" + +class DT_Polytope : public DT_Convex { +public: + DT_Polytope() {} + DT_Polytope(const DT_VertexBase *base, DT_Count count, const DT_Index *indices) + : m_base(base), + m_index(count, indices) + {} + + virtual MT_BBox bbox() const; + virtual MT_Scalar supportH(const MT_Vector3& v) const; + virtual MT_Point3 support(const MT_Vector3& v) const; + + MT_Point3 operator[](int i) const { return (*m_base)[m_index[i]]; } + DT_Count numVerts() const { return m_index.size(); } + +protected: + const DT_VertexBase *m_base; + DT_IndexArray m_index; +}; + +#endif diff --git a/extern/solid/src/convex/DT_Shape.h b/extern/solid/src/convex/DT_Shape.h new file mode 100755 index 00000000000..d48942fe515 --- /dev/null +++ b/extern/solid/src/convex/DT_Shape.h @@ -0,0 +1,72 @@ +/* + * 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. + */ + +#ifndef DT_SHAPE_H +#define DT_SHAPE_H + +#include <algorithm> + +#include "MT_BBox.h" + +#include "MT_Transform.h" + +class DT_Object; + +enum DT_ShapeType { + COMPLEX, + CONVEX +}; + +class DT_Shape { +public: + virtual ~DT_Shape() {} + virtual DT_ShapeType getType() const = 0; + virtual MT_BBox bbox(const MT_Transform& t, MT_Scalar margin) const = 0; + virtual bool ray_cast(const MT_Point3& source, const MT_Point3& target, MT_Scalar& param, MT_Vector3& normal) const = 0; + +protected: + DT_Shape() {} +}; + +typedef bool (*Intersect)(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&); + +typedef bool (*Common_point)(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&, MT_Point3&, MT_Point3&); + +typedef bool (*Penetration_depth)(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&, MT_Point3&, MT_Point3&); + +typedef MT_Scalar (*Closest_points)(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&, MT_Point3&); + +#endif + + + + + diff --git a/extern/solid/src/convex/DT_Sphere.cpp b/extern/solid/src/convex/DT_Sphere.cpp new file mode 100755 index 00000000000..3f2443dcf53 --- /dev/null +++ b/extern/solid/src/convex/DT_Sphere.cpp @@ -0,0 +1,90 @@ +/* + * 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_Sphere.h" +#include "GEN_MinMax.h" + +MT_Scalar DT_Sphere::supportH(const MT_Vector3& v) const +{ + return m_radius * v.length(); +} + +MT_Point3 DT_Sphere::support(const MT_Vector3& v) const +{ + MT_Scalar s = v.length(); + + if (s > MT_Scalar(0.0)) + { + s = m_radius / s; + return MT_Point3(v[0] * s, v[1] * s, v[2] * s); + } + else + { + return MT_Point3(m_radius, MT_Scalar(0.0), MT_Scalar(0.0)); + } +} + +bool DT_Sphere::ray_cast(const MT_Point3& source, const MT_Point3& target, + MT_Scalar& param, MT_Vector3& normal) const +{ + MT_Vector3 r = target - source; + MT_Scalar delta = -source.dot(r); + MT_Scalar r_length2 = r.length2(); + MT_Scalar sigma = delta * delta - r_length2 * (source.length2() - m_radius * m_radius); + + if (sigma >= MT_Scalar(0.0)) + // The line trough source and target intersects the sphere. + { + MT_Scalar sqrt_sigma = MT_sqrt(sigma); + // We need only the sign of lambda2, so the division by the positive + // r_length2 can be left out. + MT_Scalar lambda2 = (delta + sqrt_sigma) /* / r_length2 */ ; + if (lambda2 >= MT_Scalar(0.0)) + // The ray points at the sphere + { + MT_Scalar lambda1 = (delta - sqrt_sigma) / r_length2; + if (lambda1 <= param) + // The ray hits the sphere, since + // [lambda1, lambda2] overlaps [0, param]. + { + if (lambda1 > MT_Scalar(0.0)) + { + param = lambda1; + normal = (source + r * lambda1) / m_radius; + // NB: division by m_radius to normalize the normal. + } + else + { + param = MT_Scalar(0.0); + normal.setValue(MT_Scalar(0.0), MT_Scalar(0.0), MT_Scalar(0.0)); + } + + return true; + } + } + } + + return false; +} + + diff --git a/extern/solid/src/convex/DT_Sphere.h b/extern/solid/src/convex/DT_Sphere.h new file mode 100755 index 00000000000..92386a66f3a --- /dev/null +++ b/extern/solid/src/convex/DT_Sphere.h @@ -0,0 +1,43 @@ +/* + * 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. + */ + +#ifndef DT_SPHERE_H +#define DT_SPHERE_H + +#include "DT_Convex.h" + +class DT_Sphere : public DT_Convex { +public: + DT_Sphere(MT_Scalar radius) : m_radius(radius) {} + + virtual MT_Scalar supportH(const MT_Vector3& v) const; + virtual MT_Point3 support(const MT_Vector3& v) const; + + virtual bool ray_cast(const MT_Point3& source, const MT_Point3& target, + MT_Scalar& param, MT_Vector3& normal) const; + +protected: + MT_Scalar m_radius; +}; + +#endif diff --git a/extern/solid/src/convex/DT_Transform.h b/extern/solid/src/convex/DT_Transform.h new file mode 100755 index 00000000000..a976d48d22b --- /dev/null +++ b/extern/solid/src/convex/DT_Transform.h @@ -0,0 +1,53 @@ +/* + * 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. + */ + +#ifndef DT_TRANSFORM_H +#define DT_TRANSFORM_H + +#include "DT_Convex.h" + +class DT_Transform : public DT_Convex { +public: + DT_Transform(const MT_Transform& xform, const DT_Convex& child) : + m_xform(xform), + m_child(child) + {} + + virtual MT_Scalar supportH(const MT_Vector3& v) const + { + return m_child.supportH(v * m_xform.getBasis()) + + v.dot(m_xform.getOrigin()); + } + + virtual MT_Point3 support(const MT_Vector3& v) const + { + return m_xform(m_child.support(v * m_xform.getBasis())); + } + +private: + const MT_Transform& m_xform; + const DT_Convex& m_child; +}; + + +#endif diff --git a/extern/solid/src/convex/DT_Triangle.cpp b/extern/solid/src/convex/DT_Triangle.cpp new file mode 100755 index 00000000000..1917910b39d --- /dev/null +++ b/extern/solid/src/convex/DT_Triangle.cpp @@ -0,0 +1,96 @@ +/* + * 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. + */ + +//#define BACKFACE_CULLING + +#include "DT_Triangle.h" + +MT_BBox DT_Triangle::bbox() const +{ + return MT_BBox((*this)[0]).hull((*this)[1]).hull((*this)[2]); +} + +MT_Scalar DT_Triangle::supportH(const MT_Vector3& v) const +{ + return GEN_max(GEN_max(v.dot((*this)[0]), v.dot((*this)[1])), v.dot((*this)[2])); +} + +MT_Point3 DT_Triangle::support(const MT_Vector3& v) const +{ + MT_Vector3 dots(v.dot((*this)[0]), v.dot((*this)[1]), v.dot((*this)[2])); + + return (*this)[dots.maxAxis()]; +} + +bool DT_Triangle::ray_cast(const MT_Point3& source, const MT_Point3& target, + MT_Scalar& param, MT_Vector3& normal) const +{ + MT_Vector3 d1 = (*this)[1] - (*this)[0]; + MT_Vector3 d2 = (*this)[2] - (*this)[0]; + MT_Vector3 n = d1.cross(d2); + MT_Vector3 r = target - source; + MT_Scalar delta = -r.dot(n); + + MT_Scalar rounding_error = GEN_max(GEN_max(MT_abs(n[0]), MT_abs(n[1])), MT_abs(n[2])) * MT_EPSILON; + +#ifdef BACKFACE_CULLING + if (delta > rounding_error) +#else + if (MT_abs(delta) > rounding_error) +#endif + // The ray is not parallel to the triangle's plane. + // (Coplanar rays are ignored.) + { + MT_Vector3 b = source - (*this)[0]; + MT_Scalar lambda = b.dot(n) / delta; + + if (MT_Scalar(0.0) <= lambda && lambda <= param) + // The ray intersects the triangle's plane. + { + MT_Vector3 u = b.cross(r); + MT_Scalar mu1 = d2.dot(u) / delta; + + if (MT_Scalar(0.0) <= mu1 && mu1 <= MT_Scalar(1.0)) + { + MT_Scalar mu2 = -d1.dot(u) / delta; + + if (MT_Scalar(0.0) <= mu2 && mu1 + mu2 <= MT_Scalar(1.0)) + // The ray intersects the triangle. + { + param = lambda; + // Return a normal that points at the source. +#ifdef BACKFACE_CULLING + normal = n; +#else + normal = delta > MT_Scalar(0.0) ? n : -n; +#endif + return true; + } + } + } + } + + return false; +} + + diff --git a/extern/solid/src/convex/DT_Triangle.h b/extern/solid/src/convex/DT_Triangle.h new file mode 100755 index 00000000000..4192b5629ac --- /dev/null +++ b/extern/solid/src/convex/DT_Triangle.h @@ -0,0 +1,63 @@ +/* + * 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. + */ + +#ifndef DT_TRIANGLE_H +#define DT_TRIANGLE_H + +#include "SOLID_types.h" + +#include "DT_Convex.h" +#include "DT_IndexArray.h" +#include "DT_VertexBase.h" + +class DT_Triangle : public DT_Convex { +public: + DT_Triangle(const DT_VertexBase *base, DT_Index i0, DT_Index i1, DT_Index i2) : + m_base(base) + { + m_index[0] = i0; + m_index[1] = i1; + m_index[2] = i2; + } + + DT_Triangle(const DT_VertexBase *base, const DT_Index *index) : + m_base(base) + { + m_index[0] = index[0]; + m_index[1] = index[1]; + m_index[2] = index[2]; + } + + virtual MT_BBox bbox() const; + virtual MT_Scalar supportH(const MT_Vector3& v) const; + virtual MT_Point3 support(const MT_Vector3& v) const; + virtual bool ray_cast(const MT_Point3& source, const MT_Point3& target, MT_Scalar& lambda, MT_Vector3& normal) const; + + MT_Point3 operator[](int i) const { return (*m_base)[m_index[i]]; } + +private: + const DT_VertexBase *m_base; + DT_Index m_index[3]; +}; + +#endif diff --git a/extern/solid/src/convex/DT_VertexBase.h b/extern/solid/src/convex/DT_VertexBase.h new file mode 100755 index 00000000000..37646fdd935 --- /dev/null +++ b/extern/solid/src/convex/DT_VertexBase.h @@ -0,0 +1,84 @@ +/* + * 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. + */ + +#ifndef DT_VERTEXBASE_H +#define DT_VERTEXBASE_H + +#include "MT_Point3.h" + +#include <vector> + +class DT_Complex; + +typedef std::vector<DT_Complex *>DT_ComplexList; + +class DT_VertexBase { +public: + explicit DT_VertexBase(const void *base = 0, DT_Size stride = 0, bool owner = false) : + m_base((char *)base), + m_stride(stride ? stride : 3 * sizeof(DT_Scalar)), + m_owner(owner) + {} + + ~DT_VertexBase() + { + if (m_owner) + { + delete [] m_base; + } + } + + MT_Point3 operator[](DT_Index i) const + { + return MT_Point3(reinterpret_cast<DT_Scalar *>(m_base + i * m_stride)); + } + + void setPointer(const void *base, bool owner = false) + { + m_base = (char *)base; + m_owner = owner; + } + + const void *getPointer() const { return m_base; } + bool isOwner() const { return m_owner; } + + void addComplex(DT_Complex *complex) const { m_complexList.push_back(complex); } + void removeComplex(DT_Complex *complex) const + { + DT_ComplexList::iterator it = std::find(m_complexList.begin(), m_complexList.end(), complex); + if (it != m_complexList.end()) + { + m_complexList.erase(it); + } + } + + const DT_ComplexList& getComplexList() const { return m_complexList; } + +private: + char *m_base; + DT_Size m_stride; + bool m_owner; + mutable DT_ComplexList m_complexList; +}; + +#endif |