SSVTreeCollider.cpp
Go to the documentation of this file.
1 #include <iostream>
2 #include "SSVTreeCollider.h"
3 #include "DistFuncs.h"
4 
5 static bool debug = false;
6 
8 {
9 }
10 
12  float& minD, Point &point0, Point&point1,
13  const Matrix4x4* world0, const Matrix4x4* world1)
14 {
15  // Checkings
16  if(!cache.Model0 || !cache.Model1) return false;
17  if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false;
18  if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false;
19 
20  // Checkings
21  if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false;
22 
23  // Simple double-dispatch
24  const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
25  const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
26  Distance(T0, T1, world0, world1, &cache, minD, point0, point1);
27  return true;
28 }
29 
31  const AABBCollisionTree* tree1,
32  const Matrix4x4* world0, const Matrix4x4* world1,
33  Pair* cache, float& minD, Point &point0, Point&point1)
34 {
35  if (debug) std::cout << "Distance()" << std::endl;
36  // Init collision query
37  InitQuery(world0, world1);
38 
39  // Compute initial value using temporal coherency
40  // todo : cache should be used
41 
42  const AABBCollisionNode *n;
43  for (unsigned int i=0; i<tree0->GetNbNodes(); i++){
44  n = tree0->GetNodes()+i;
45  if (n->IsLeaf()){
46  mId0 = n->GetPrimitive();
47  break;
48  }
49  }
50  for (unsigned int i=0; i<tree1->GetNbNodes(); i++){
51  n = tree1->GetNodes()+i;
52  if (n->IsLeaf()){
53  mId1 = n->GetPrimitive();
54  break;
55  }
56  }
57  Point p0, p1;
58  minD = PrimDist(mId0, mId1, p0, p1);
59 
60  // Perform distance computation
61  _Distance(tree0->GetNodes(), tree1->GetNodes(), minD, p0, p1);
62 
63  // transform points
64  TransformPoint4x3(point0, p0, *world1);
65  TransformPoint4x3(point1, p1, *world1);
66 
67  // update cache
68  cache->id0 = mId0;
69  cache->id1 = mId1;
70 }
71 
72 bool SSVTreeCollider::Collide(BVTCache& cache, double tolerance,
73  const Matrix4x4* world0, const Matrix4x4* world1)
74 {
75  // Checkings
76  if(!cache.Model0 || !cache.Model1) return false;
77  if(cache.Model0->HasLeafNodes()!=cache.Model1->HasLeafNodes()) return false;
78  if(cache.Model0->IsQuantized()!=cache.Model1->IsQuantized()) return false;
79 
80  // Checkings
81  if(!Setup(cache.Model0->GetMeshInterface(), cache.Model1->GetMeshInterface())) return false;
82 
83  // Simple double-dispatch
84  const AABBCollisionTree* T0 = (const AABBCollisionTree*)cache.Model0->GetTree();
85  const AABBCollisionTree* T1 = (const AABBCollisionTree*)cache.Model1->GetTree();
86  return Collide(T0, T1, world0, world1, &cache, tolerance);
87 }
88 
90  const AABBCollisionTree* tree1,
91  const Matrix4x4* world0, const Matrix4x4* world1,
92  Pair* cache, double tolerance)
93 {
94  // Init collision query
95  InitQuery(world0, world1);
96 
97  // todo : cache should be used
98 
99  // Perform collision detection
100  if (_Collide(tree0->GetNodes(), tree1->GetNodes(), tolerance)){
101  // update cache
102  cache->id0 = mId0;
103  cache->id1 = mId1;
104  return true;
105  }
106 
107  return false;
108 }
109 
111  const AABBCollisionNode *b1)
112 {
114  t1 = b0->mAABB.mType;
115  t2 = b1->mAABB.mType;
117  return PssPssDist(b0->mAABB.mRadius, b0->mAABB.mCenter,
118  b1->mAABB.mRadius, b1->mAABB.mCenter);
119  }else if (t1 == CollisionAABB::SSV_PSS && t2 == CollisionAABB::SSV_LSS){
120  return PssLssDist(b0->mAABB.mRadius, b0->mAABB.mCenter,
121  b1->mAABB.mRadius, b1->mAABB.mPoint0, b1->mAABB.mPoint1);
122  }else if (t1 == CollisionAABB::SSV_LSS && t2 == CollisionAABB::SSV_PSS){
123  return LssPssDist(b0->mAABB.mRadius, b0->mAABB.mPoint0, b0->mAABB.mPoint1,
124  b1->mAABB.mRadius, b1->mAABB.mCenter);
125  }else if (t1 == CollisionAABB::SSV_LSS && t2 == CollisionAABB::SSV_LSS){
126  return PssPssDist(sqrtf(b0->GetSize()), b0->mAABB.mCenter,
127  sqrtf(b1->GetSize()), b1->mAABB.mCenter);
128  }else{
129  std::cerr << "this ssv combination is not supported" << std::endl;
130  }
131 }
132 
133 float SSVTreeCollider::PssPssDist(float r0, const Point& center0, float r1, const Point& center1)
134 {
135  Point c0;
136  TransformPoint(c0, center0, mR0to1, mT0to1);
137  return (center1-c0).Magnitude() - r0 - r1;
138 }
139 
140 float SSVTreeCollider::PssLssDist(float r0, const Point& center0, float r1, const Point& point0, const Point& point1)
141 {
142  Point c0;
143  TransformPoint(c0, center0, mR0to1, mT0to1);
144  float d = PointSegDist(c0, point0, point1);
145  return d - r0 - r1;
146 }
147 
148 float SSVTreeCollider::LssPssDist(float r0, const Point& point0, const Point& point1, float r1, const Point& center0)
149 {
150  Point p0, p1;
151  TransformPoint(p0, point0, mR0to1, mT0to1);
152  TransformPoint(p1, point1, mR0to1, mT0to1);
153  float d = PointSegDist(center0, p0, p1);
154  return d - r0 - r1;
155 }
156 
157 float SSVTreeCollider::LssLssDist(float r0, const Point& point0, const Point& point1, float r1, const Point& point2, const Point& point3)
158 {
159  Point p0, p1;
160  TransformPoint(p0, point0, mR0to1, mT0to1);
161  TransformPoint(p1, point1, mR0to1, mT0to1);
162  float d = SegSegDist(p0, p1, point2, point3);
163  return d - r0 - r1;
164 }
165 
167  float& minD, Point& point0, Point& point1)
168 {
169  if (debug) std::cout << "_Distance()" << std::endl;
170 
171  mNowNode0 = b0;
172  mNowNode1 = b1;
173  float d;
174 
175  // Perform BV-BV distance test
176  d = SsvSsvDist(b0, b1);
177 
178  if(d > minD) return;
179 
180  if(b0->IsLeaf() && b1->IsLeaf()) {
181  Point p0, p1;
182  d = PrimDist(b0->GetPrimitive(), b1->GetPrimitive(), p0, p1);
183  if (d < minD){
184  minD = d;
185  point0 = p0;
186  point1 = p1;
187  mId0 = b0->GetPrimitive();
188  mId1 = b1->GetPrimitive();
189  }
190  return;
191  }
192 
193  if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
194  {
195  _Distance(b0->GetNeg(), b1, minD, point0, point1);
196  _Distance(b0->GetPos(), b1, minD, point0, point1);
197  }
198  else
199  {
200  _Distance(b0, b1->GetNeg(), minD, point0, point1);
201  _Distance(b0, b1->GetPos(), minD, point0, point1);
202  }
203 }
204 
206  double tolerance)
207 {
208  mNowNode0 = b0;
209  mNowNode1 = b1;
210  float d;
211 
212  // Perform BV-BV distance test
213  d = SsvSsvDist(b0, b1);
214 
215  if(d > tolerance) return false;
216 
217  if(b0->IsLeaf() && b1->IsLeaf()) {
218  Point p0, p1;
219  d = PrimDist(b0->GetPrimitive(), b1->GetPrimitive(), p0, p1);
220  if (d <= tolerance){
221  mId0 = b0->GetPrimitive();
222  mId1 = b1->GetPrimitive();
223  return true;
224  }else{
225  return false;
226  }
227  }
228 
229  if(b1->IsLeaf() || (!b0->IsLeaf() && (b0->GetSize() > b1->GetSize())))
230  {
231  if (_Collide(b0->GetNeg(), b1, tolerance)) return true;
232  if (_Collide(b0->GetPos(), b1, tolerance)) return true;
233  }
234  else
235  {
236  if (_Collide(b0, b1->GetNeg(), tolerance)) return true;
237  if (_Collide(b0, b1->GetPos(), tolerance)) return true;
238  }
239  return false;
240 }
241 
242 float SSVTreeCollider::PrimDist(udword id0, udword id1, Point& point0, Point& point1)
243 {
244  // Request vertices from the app
245  VertexPointers VP0;
246  VertexPointers VP1;
247  mIMesh0->GetTriangle(VP0, id0);
248  mIMesh1->GetTriangle(VP1, id1);
249 
250  // Modified by S-cubed, Inc.
251  // Transform from space 0 (old : 1) to space 1 (old : 0)
252  // CD では変換が逆なのであわせる。
253  Point u0,u1,u2;
254  TransformPoint(u0, *VP0.Vertex[0], mR0to1, mT0to1);
255  TransformPoint(u1, *VP0.Vertex[1], mR0to1, mT0to1);
256  TransformPoint(u2, *VP0.Vertex[2], mR0to1, mT0to1);
257 
258  // Perform triangle-triangle distance test
259  return TriTriDist(u0, u1, u2,
260  *VP1.Vertex[0], *VP1.Vertex[1], *VP1.Vertex[2],
261  point0, point1);
262 }
bool _Collide(const AABBCollisionNode *b0, const AABBCollisionNode *b1, double tolerance)
inline_ float GetSize() const
Definition: Opcode.h:82
virtual inline_ void InitQuery()
Definition: Opcode.h:174
inline_ void GetTriangle(VertexPointers &vp, udword index) const
Definition: Opcode.h:119
Matrix3x3 mR0to1
Rotation from object0 to object1.
Definition: Opcode.h:199
float PssLssDist(float r0, const Point &center0, float r1, const Point &point0, const Point &point1)
compute distance between PSS(Point Swept Sphere) and LSS(Line Swept Sphere)
static bool debug
A generic couple structure.
Definition: OPC_IceHook.h:17
float LssPssDist(float r0, const Point &point0, const Point &point1, float r1, const Point &center0)
compute distance between PSS(Point Swept Sphere) and LSS(Line Swept Sphere)
float SegSegDist(const Point &u0, const Point &u, const Point &v0, const Point &v, Point &cp0, Point &cp1)
compute the minimum distance and the closest points between two line segments
Definition: DistFuncs.cpp:24
const Point * Vertex[3]
Definition: Opcode.h:26
float LssLssDist(float r0, const Point &point0, const Point &point1, float r1, const Point &point2, const Point &point3)
compute distance between LSS(Line Swept Sphere)s
png_uint_32 i
Definition: png.h:2735
const Model * Model1
Model for second object.
Definition: Opcode.h:61
udword id1
Second index of the pair.
Definition: OPC_IceHook.h:23
inline_ const AABBOptimizedTree * GetTree() const
Definition: Opcode.h:99
unsigned int udword
sizeof(udword) must be 4
Definition: IceTypes.h:65
inline_ BOOL Setup(const MeshInterface *mi0, const MeshInterface *mi1)
Definition: Opcode.h:247
float PssPssDist(float r0, const Point &center0, float r1, const Point &center1)
compute distance between PSS(Point Swept Sphere)
inline_ BOOL IsQuantized() const
Definition: Opcode.h:132
const MeshInterface * mIMesh1
User-defined mesh interface for object1.
Definition: Opcode.h:192
inline_ void TransformPoint4x3(Point &dest, const Point &source, const Matrix4x4 &rot)
Quickly rotates & translates a vector, using the 4x3 part of a 4x4 matrix.
Definition: IceMatrix4x4.h:437
float SsvSsvDist(const AABBCollisionNode *b0, const AABBCollisionNode *b1)
compute distance between SSV(Swept Sphere Volume)s
const AABBCollisionNode * mNowNode0
Definition: Opcode.h:212
udword id0
First index of the pair.
Definition: OPC_IceHook.h:22
float PointSegDist(const Point &P, const Point &u0, const Point &u1)
compute distance between a point and a line segment
Definition: DistFuncs.cpp:115
float PrimDist(udword id0, udword id1, Point &point0, Point &point1)
compute distance between primitives(triangles)
const Model * Model0
Model for first object.
Definition: Opcode.h:60
inline_ udword GetNbNodes() const
Definition: Opcode.h:187
const AABBCollisionNode * mNowNode1
Definition: Opcode.h:213
bool Distance(BVTCache &cache, float &minD, Point &point0, Point &point1, const Matrix4x4 *world0=null, const Matrix4x4 *world1=null)
compute the minimum distance and the closest points
inline_ void TransformPoint(Point &dest, const Point &source, const Matrix3x3 &rot, const Point &trans)
Quickly rotates & translates a vector.
Definition: OPC_Common.h:118
void _Distance(const AABBCollisionNode *b0, const AABBCollisionNode *b1, float &minD, Point &point0, Point &point1)
float TriTriDist(const Point &U0, const Point &U1, const Point &U2, const Point &V0, const Point &V1, const Point &V2, Point &cp0, Point &cp1)
compute the minimum distance and the closest points between two triangles
Definition: DistFuncs.cpp:154
Point mT0to1
Translation from object0 to object1.
Definition: Opcode.h:201
inline_ BOOL HasLeafNodes() const
Definition: Opcode.h:124
bool Collide(BVTCache &cache, double tolerance, const Matrix4x4 *world0=null, const Matrix4x4 *world1=null)
detect collision between links.
inline_ const MeshInterface * GetMeshInterface() const
Definition: Opcode.h:156
SSVTreeCollider()
constructor
const MeshInterface * mIMesh0
User-defined mesh interface for object0.
Definition: Opcode.h:191


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:41