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
|
// © 2004 Janek Kozicki <cosurgi@berlios.de>
// © 2007 Bruno Chareyre <bruno.chareyre@grenoble-inp.fr>
// © 2008 Václav Šmilauer <eudoxos@arcig.cz>
#include "Ig2_Sphere_Sphere_ScGeom.hpp"
#include <lib/base/Math.hpp>
#include <core/InteractionLoop.hpp>
#include <core/Omega.hpp>
#include <core/Scene.hpp>
#include <pkg/common/Sphere.hpp>
#include <pkg/dem/ScGeom.hpp>
namespace yade { // Cannot have #include directive inside.
bool Ig2_Sphere_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)
{
TIMING_DELTAS_START();
const Se3r& se31 = state1.se3;
const Se3r& se32 = state2.se3;
const Sphere *s1 = static_cast<Sphere*>(cm1.get()), *s2 = static_cast<Sphere*>(cm2.get());
Vector3r normal = (se32.position + shift2) - se31.position;
if (!c->isReal() && !force) { //don't fast-check distance if geometry will be updated anyway
Real penetrationDepthSq = pow(interactionDetectionFactor * (s1->radius + s2->radius), 2) - normal.squaredNorm();
if (penetrationDepthSq < 0) {
TIMING_DELTAS_CHECKPOINT("Ig2_Sphere_Sphere_ScGeom");
return false;
}
}
shared_ptr<ScGeom> scm;
bool isNew = !c->geom;
if (!isNew) scm = YADE_PTR_CAST<ScGeom>(c->geom);
else {
scm = shared_ptr<ScGeom>(new ScGeom());
c->geom = scm;
}
Real norm = normal.norm();
normal /= norm; // normal is unit vector now
#ifdef YADE_DEBUG
if (norm == 0)
throw runtime_error(("Zero distance between spheres #" + boost::lexical_cast<string>(c->getId1()) + " and #"
+ boost::lexical_cast<string>(c->getId2()) + ".")
.c_str());
#endif
Real penetrationDepth = s1->radius + s2->radius - norm;
scm->contactPoint = se31.position + (s1->radius - 0.5 * penetrationDepth) * normal; //0.5*(pt1+pt2);
scm->penetrationDepth = penetrationDepth;
scm->radius1 = s1->radius;
scm->radius2 = s2->radius;
scm->precompute(state1, state2, scene, c, normal, isNew, shift2, avoidGranularRatcheting);
TIMING_DELTAS_CHECKPOINT("Ig2_Sphere_Sphere_ScGeom");
return true;
}
bool Ig2_Sphere_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)
{
return go(cm1, cm2, state2, state1, -shift2, force, c);
}
YADE_PLUGIN((Ig2_Sphere_Sphere_ScGeom));
bool Ig2_Sphere_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_Sphere_Sphere_ScGeom::go(cm1, cm2, state1, state2, shift2, force, c)) { //the 3 DOFS from ScGeom are updated here
if (isNew) { //generate a 6DOF interaction from the 3DOF one generated by Ig2_Sphere_Sphere_ScGeom
shared_ptr<ScGeom6D> sc(new ScGeom6D());
*(YADE_PTR_CAST<ScGeom>(sc)) = *(YADE_PTR_CAST<ScGeom>(c->geom));
c->geom = sc;
}
if (updateRotations) YADE_PTR_CAST<ScGeom6D>(c->geom)->precomputeRotations(state1, state2, isNew, creep);
return true;
} else
return false;
}
bool Ig2_Sphere_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)
{
return go(cm1, cm2, state2, state1, -shift2, force, c);
}
YADE_PLUGIN((Ig2_Sphere_Sphere_ScGeom6D));
} // namespace yade
|
