1 // =============================================================================
2 // PROJECT CHRONO - http://projectchrono.org
3 //
4 // Copyright (c) 2014 projectchrono.org
5 // All rights reserved.
6 //
7 // Use of this source code is governed by a BSD-style license that can be found
8 // in the LICENSE file at the top level of the distribution and at
9 // http://projectchrono.org/license-chrono.txt.
10 //
11 // =============================================================================
12
13 #include <cstdio>
14 #include <cstring>
15
16 #include "chrono/collision/edgetempest/ChCMatVec.h"
17 #include "chrono/collision/edgetempest/ChCGetTime.h"
18 #include "chrono/collision/edgetempest/ChCOBBcollider.h"
19 #include "chrono/collision/edgetempest/ChCGeometryCollider.h"
20
21 #include "chrono/core/ChTransform.h"
22
23 namespace chrono {
24 namespace collision {
25
CHOBBcollider()26 CHOBBcollider::CHOBBcollider() {
27 }
28
~CHOBBcollider()29 CHOBBcollider::~CHOBBcollider() {
30 }
31
CollideRecurse(ChMatrix33<> & boR,Vector & boT,CHOBBTree * o1,int b1,CHOBBTree * o2,int b2,eCollMode flag)32 void CHOBBcollider::CollideRecurse(ChMatrix33<>& boR,
33 Vector& boT,
34 CHOBBTree* o1,
35 int b1,
36 CHOBBTree* o2,
37 int b2,
38 eCollMode flag) {
39 this->num_bv_tests++;
40
41 CHOBB* box1 = o1->child(b1);
42 CHOBB* box2 = o2->child(b2);
43
44 // first thing, see if we're overlapping
45 if (!CHOBB::OBB_Overlap(boR, boT, box1, box2))
46 return;
47
48 // if we are, see if we test geometries next
49
50 int l1 = box1->IsLeaf();
51 int l2 = box2->IsLeaf();
52
53 //
54 // CASE TWO LEAVES
55 //
56
57 if (l1 && l2) {
58 this->num_geo_tests++;
59
60 // transform the points in b2 into space of b1, then compare
61
62 geometry::ChGeometry* mgeo1 = o1->geometries[box1->GetGeometryIndex()];
63 geometry::ChGeometry* mgeo2 = o2->geometries[box2->GetGeometryIndex()];
64
65 bool just_intersect = false;
66 if (flag == ChNarrowPhaseCollider::ChC_FIRST_CONTACT)
67 just_intersect = true;
68
69 ChGeometryCollider::ComputeCollisions(*mgeo1, &this->R1, &this->T1, *mgeo2, &this->R2, &this->T2, *this,
70 &this->R, &this->T, just_intersect);
71 return;
72 }
73
74 // we don't, so decide whose children to visit next
75
76 double sz1 = box1->GetSize();
77 double sz2 = box2->GetSize();
78
79 ChMatrix33<> Rc;
80 Vector Tc;
81
82 if (l2 || (!l1 && (sz1 > sz2))) {
83 int c1 = box1->GetFirstChildIndex();
84 int c2 = box1->GetSecondChildIndex();
85
86 Rc = o1->child(c1)->Rot.transpose() * boR;
87
88 Tc = ChTransform<>::TransformParentToLocal(boT, o1->child(c1)->To, o1->child(c1)->Rot);
89
90 CollideRecurse(Rc, Tc, o1, c1, o2, b2, flag);
91
92 if ((flag == ChC_FIRST_CONTACT) && (this->GetNumPairs() > 0))
93 return;
94
95 Rc = o1->child(c2)->Rot.transpose() * boR;
96
97 Tc = ChTransform<>::TransformParentToLocal(boT, o1->child(c2)->To, o1->child(c2)->Rot);
98
99 CollideRecurse(Rc, Tc, o1, c2, o2, b2, flag);
100 } else {
101 int c1 = box2->GetFirstChildIndex();
102 int c2 = box2->GetSecondChildIndex();
103
104 Rc = boR * o2->child(c1)->Rot;
105
106 Tc = ChTransform<>::TransformLocalToParent(o2->child(c1)->To, boT, boR);
107
108 CollideRecurse(Rc, Tc, o1, b1, o2, c1, flag);
109
110 if ((flag == ChC_FIRST_CONTACT) && (this->GetNumPairs() > 0))
111 return;
112
113 Rc = boR * o2->child(c2)->Rot;
114
115 Tc = ChTransform<>::TransformLocalToParent(o2->child(c2)->To, boT, boR);
116
117 CollideRecurse(Rc, Tc, o1, b1, o2, c2, flag);
118 }
119 }
120
121 //
122 // PERFORM COLLISION DETECTION
123 //
124
ComputeCollisions(ChMatrix33<> & R1,Vector T1,ChCollisionTree * oc1,ChMatrix33<> & R2,Vector T2,ChCollisionTree * oc2,eCollMode flag)125 ChNarrowPhaseCollider::eCollSuccess CHOBBcollider::ComputeCollisions(ChMatrix33<>& R1,
126 Vector T1,
127 ChCollisionTree* oc1,
128 ChMatrix33<>& R2,
129 Vector T2,
130 ChCollisionTree* oc2,
131 eCollMode flag) {
132 double t1 = GetTime();
133
134 // INHERIT parent class behavior
135
136 if (ChNarrowPhaseCollider::ComputeCollisions(R1, T1, oc1, R2, T2, oc2, flag) != ChC_RESULT_OK)
137 return ChC_RESULT_GENERICERROR;
138
139 // Downcasting
140 CHOBBTree* o1 = (CHOBBTree*)oc1;
141 CHOBBTree* o2 = (CHOBBTree*)oc2;
142
143 // clear the stats
144
145 this->num_bv_tests = 0;
146 this->num_geo_tests = 0;
147
148 // compute the transform from o1->child(0) to o2->child(0)
149
150 static ChMatrix33<> Rtemp;
151 static ChMatrix33<> bR;
152 static Vector bT;
153 static Vector Ttemp;
154
155 Rtemp = this->R * o2->child(0)->Rot; // MxM(Rtemp,this->R,o2->child(0)->R);
156 bR = o1->child(0)->Rot * Rtemp; // MTxM(R,o1->child(0)->R,Rtemp);
157
158 Ttemp = ChTransform<>::TransformLocalToParent(o2->child(0)->To, this->T,
159 this->R); // MxVpV(Ttemp,this->R,o2->child(0)->To,this->T);
160 Ttemp = Vsub(Ttemp, o1->child(0)->To); // VmV(Ttemp,Ttemp,o1->child(0)->To);
161
162 bT = o1->child(0)->Rot.transpose() * Ttemp; // MTxV(T,o1->child(0)->R,Ttemp);
163
164 // now start with both top level BVs
165
166 CollideRecurse(bR, bT, o1, 0, o2, 0, flag);
167
168 double t2 = GetTime();
169 this->query_time_secs = t2 - t1;
170
171 return ChC_RESULT_OK;
172 }
173
174 } // end namespace collision
175 } // end namespace chrono
176