1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
|
/*************************************************************************
* Copyright (C) 2004 by Olivier Galizzi *
* olivier.galizzi@imag.fr *
* Copyright (C) 2004 by Janek Kozicki *
* cosurgi@berlios.de *
* Copyright (C) 2006 by Bruno Chareyre *
* bruno.chareyre@grenoble-inp.fr *
* *
* This program is free software; it is licensed under the terms of the *
* GNU General Public License v2 or later. See file LICENSE for details. *
*************************************************************************/
#include "Ig2_Box_Sphere_ScGeom.hpp"
#include <lib/base/Math.hpp>
#include <core/Scene.hpp>
#include <pkg/common/Box.hpp>
#include <pkg/common/Sphere.hpp>
#include <pkg/dem/ScGeom.hpp>
namespace yade { // Cannot have #include directive inside.
bool Ig2_Box_Sphere_ScGeom::go(
const shared_ptr<Shape>& cm1,
const shared_ptr<Shape>& cm2,
const State& state1,
const State& state2,
const Vector3r& shift2,
const bool& force,
const shared_ptr<Interaction>& c)
{
const Se3r& se31 = state1.se3;
const Se3r& se32 = state2.se3;
bool inside = true;
Vector3r pt1, pt2, normal;
Real depth;
Box* obb = static_cast<Box*>(cm1.get());
Sphere* s = static_cast<Sphere*>(cm2.get());
Vector3r extents = obb->extents;
// FIXME: do we need rotation matrix? Can't quaternion do just fine?
Matrix3r boxAxisT = se31.orientation.toRotationMatrix();
Matrix3r boxAxis = boxAxisT.transpose();
Vector3r relPos21 = se32.position + shift2 - se31.position; // relative position of centroids
// cOnBox_boxLocal is the sphere centroid (in box-local coordinates), but projected onto box if it is outside.
// _boxLocal means that ROTATION is local and origin is in box's origin
Vector3r cOnBox_boxLocal = boxAxis * relPos21;
if (cOnBox_boxLocal[0] < -extents[0]) {
cOnBox_boxLocal[0] = -extents[0];
inside = false;
}
if (cOnBox_boxLocal[0] > extents[0]) {
cOnBox_boxLocal[0] = extents[0];
inside = false;
}
if (cOnBox_boxLocal[1] < -extents[1]) {
cOnBox_boxLocal[1] = -extents[1];
inside = false;
}
if (cOnBox_boxLocal[1] > extents[1]) {
cOnBox_boxLocal[1] = extents[1];
inside = false;
}
if (cOnBox_boxLocal[2] < -extents[2]) {
cOnBox_boxLocal[2] = -extents[2];
inside = false;
}
if (cOnBox_boxLocal[2] > extents[2]) {
cOnBox_boxLocal[2] = extents[2];
inside = false;
}
const Real sphereRadiusForBox = (hertzian) ? hertzFac * s->radius : s->radius;
shared_ptr<ScGeom> scm;
if (inside) {
// sphere center inside box. find largest `cOnBox_boxLocal' value:
// minCBoxDist_index is the coordinate index that minimizes extents[minCBoxDist_index]-math::abs(cOnBox_boxLocal[minCBoxDist_index] (sphere center closest to box boundary)
// where cOnBox_boxLocal is minimal (i.e. where sphere center is closest perpendicularly to the box)
Real minCBoxDist = extents[0] - math::abs(cOnBox_boxLocal[0]);
int minCBoxDist_index = 0;
for (int i = 1; i < 3; i++) {
Real tt = extents[i] - math::abs(cOnBox_boxLocal[i]);
if (tt < minCBoxDist) {
minCBoxDist = tt;
minCBoxDist_index = i;
}
}
// contact normal aligned with box edge along largest `cOnBox_boxLocal' value
Vector3r normal_boxLocal = Vector3r(0, 0, 0);
normal_boxLocal[minCBoxDist_index] = (cOnBox_boxLocal[minCBoxDist_index] > 0) ? 1.0 : -1.0;
normal = boxAxisT * normal_boxLocal;
normal.normalize();
// se32 is sphere's se3
/*
*
* _--(pt1)-_ BOX
* +------~-----x-----~----------------+
* | / ^ \ |
* | / | (normal)*minCBoxDist |
* | | x | |
* | \ | c ≡ se32->position |
* | \ | / |
* | ~ | / SPHERE |
* | ^~~ x ~~^ |
* | (pt2) |
* +-----------------------------------+
*
*
*/
pt1 = se32.position + normal * minCBoxDist;
pt2 = se32.position - normal * s->radius;
Vector3r normal2 = pt1 - pt2;
normal2.normalize();
bool isNew = !c->geom;
if (isNew) scm = shared_ptr<ScGeom>(new ScGeom());
else
scm = YADE_PTR_CAST<ScGeom>(c->geom);
// contact point is in the middle of overlapping volumes
//(in the direction of penetration, which is normal2 to the box surface closest to sphere center) of overlapping volumes
scm->contactPoint = 0.5 * (pt1 + pt2);
// scm->normal2 = normal2;
scm->penetrationDepth = (pt1 - pt2).norm();
scm->radius1 = sphereRadiusForBox;
scm->radius2 = s->radius;
c->geom = scm;
scm->precompute(state1, state2, scene, c, normal2, isNew, shift2, true);
} else { // outside
Vector3r cOnBox_box = boxAxisT
* cOnBox_boxLocal; // projection of sphere's center on closest box surface - relative to box's origin, but GLOBAL orientation!
Vector3r cOnBox_sphere = cOnBox_box - relPos21; // same, but origin in sphere's center
depth = s->radius - cOnBox_sphere.norm();
if (-depth > s->radius * (interactionDetectionFactor - 1.) && !c->isReal() && !force) return false;
/*
* +-----------------------------------+
* | |
* | se31->position |
* | pt2 × |
* | × / cOnBox_box |
* | pt1 / |
* +------~-----×-----~----------------+
* / ^ \
* / | cOnBox_sphere
* | × |
* \ c ≡ se32->position
* \ /
* ~ /
* ^~~ ~ ~~^
*
*/
pt1 = cOnBox_box + se31.position;
cOnBox_sphere.normalize(); // we want only direction in the following
pt2 = se32.position + shift2 + cOnBox_sphere * s->radius;
bool isNew = !c->geom;
if (isNew) scm = shared_ptr<ScGeom>(new ScGeom());
else
scm = YADE_PTR_CAST<ScGeom>(c->geom);
scm->contactPoint = 0.5 * (pt1 + pt2);
scm->penetrationDepth = depth;
scm->radius1 = sphereRadiusForBox;
scm->radius2 = s->radius;
c->geom = scm;
//FIXME : NOT TESTED : do we precompute correctly if boxes are moving?
scm->precompute(state1, state2, scene, c, -cOnBox_sphere, isNew, shift2, false);
}
return true;
}
bool Ig2_Box_Sphere_ScGeom::goReverse(
const shared_ptr<Shape>& cm1,
const shared_ptr<Shape>& cm2,
const State& state1,
const State& state2,
const Vector3r& shift2,
const bool& force,
const shared_ptr<Interaction>& c)
{
c->swapOrder();
return go(cm2, cm1, state2, state1, -shift2, force, c);
}
YADE_PLUGIN((Ig2_Box_Sphere_ScGeom));
bool Ig2_Box_Sphere_ScGeom6D::go(
const shared_ptr<Shape>& cm1,
const shared_ptr<Shape>& cm2,
const State& state1,
const State& state2,
const Vector3r& shift2,
const bool& force,
const shared_ptr<Interaction>& c)
{
bool isNew = !c->geom;
if (Ig2_Box_Sphere_ScGeom::go(cm1, cm2, state1, state2, shift2, force, c)) {
if (isNew) { //generate a 6DOF interaction from the 3DOF one generated by Ig2_Box_Sphere_ScGeom
shared_ptr<ScGeom6D> sc(new ScGeom6D());
*(YADE_PTR_CAST<ScGeom>(sc)) = *(YADE_PTR_CAST<ScGeom>(c->geom));
c->geom = sc;
}
YADE_PTR_CAST<ScGeom6D>(c->geom)->precomputeRotations(state1, state2, isNew, false);
return true;
} else
return false;
}
bool Ig2_Box_Sphere_ScGeom6D::goReverse(
const shared_ptr<Shape>& cm1,
const shared_ptr<Shape>& cm2,
const State& state1,
const State& state2,
const Vector3r& shift2,
const bool& force,
const shared_ptr<Interaction>& c)
{
c->swapOrder();
return go(cm2, cm1, state2, state1, -shift2, force, c);
}
YADE_PLUGIN((Ig2_Box_Sphere_ScGeom6D));
} // namespace yade
|
