PeriDyno 1.0.0
All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros
CollisionDetector.cpp
Go to the documentation of this file.
1#include "CollisionDetector.h"
2
4
6
11
12#include "GLPointVisualModule.h"
14
15namespace dyno
16{
18
19 template<typename TDataType>
21 this->setAutoSync(true);
22
23 auto pointRender = std::make_shared<GLPointVisualModule>();
24 pointRender->varPointSize()->setValue(0.02);
25 pointRender->varBaseColor()->setValue(Color(1.0f, 0.0f, 0.0f));
26 this->stateContacts()->connect(pointRender->inPointSet());
27 this->graphicsPipeline()->pushModule(pointRender);
28
29 auto wireRender = std::make_shared<GLWireframeVisualModule>();
30 wireRender->varRenderMode()->setCurrentKey(GLWireframeVisualModule::CYLINDER);
31 this->stateNormals()->connect(wireRender->inEdgeSet());
32 this->graphicsPipeline()->pushModule(wireRender);
33 };
34
35 template<typename TDataType>
37 {
38 if (this->stateContacts()->isEmpty()) {
39 this->stateContacts()->allocate();
40 }
41
42 if (this->stateNormals()->isEmpty()) {
43 this->stateNormals()->allocate();
44 }
45
46 auto shapeA = this->getShapeA();
47 auto shapeB = this->getShapeB();
48
49 TManifold<Real> manifold;
50
51 if (shapeA->getShapeType() == BasicShapeType::CUBE && shapeB->getShapeType() == BasicShapeType::CUBE) //cube-cube
52 {
53 auto modelA = dynamic_cast<CubeModel<TDataType>*>(shapeA);
54 auto modelB = dynamic_cast<CubeModel<TDataType>*>(shapeB);
55
56 auto sA = modelA->outCube()->getValue();
57 auto sB = modelB->outCube()->getValue();
58
59 //CollisionDetection<Real>::request(manifold, sA, sB);
60 CollisionDetection<Real>::request(manifold, sA, sB, 0.01f, 0.01f);
61 }
62 else if (shapeA->getShapeType() == BasicShapeType::SPHERE && shapeB->getShapeType() == BasicShapeType::SPHERE) //sphere-sphere
63 {
64 auto modelA = dynamic_cast<SphereModel<TDataType>*>(shapeA);
65 auto modelB = dynamic_cast<SphereModel<TDataType>*>(shapeB);
66
67 auto sA = modelA->outSphere()->getValue();
68 auto sB = modelB->outSphere()->getValue();
69
70 //CollisionDetection<Real>::request(manifold, sA, sB);
71 CollisionDetection<Real>::request(manifold, sA, sB, 0.0f, 0.0f);
72 }
73 else if (shapeA->getShapeType() == BasicShapeType::CAPSULE && shapeB->getShapeType() == BasicShapeType::CAPSULE) //capsule-capsule
74 {
75 auto modelA = dynamic_cast<CapsuleModel<TDataType>*>(shapeA);
76 auto modelB = dynamic_cast<CapsuleModel<TDataType>*>(shapeB);
77
78 auto sA = modelA->outCapsule()->getValue();
79 auto sB = modelB->outCapsule()->getValue();
80
81 Segment3D segA = sA.centerline();
82 Segment3D segB = sB.centerline();
83
84
85 //CollisionDetection<Real>::request(manifold, sA, sB);
86 CollisionDetection<Real>::request(manifold, segA, segB, sA.radius, sB.radius);
87 }
88 else if (shapeA->getShapeType() == BasicShapeType::CUBE && shapeB->getShapeType() == BasicShapeType::CAPSULE) //cube-capsule
89 {
90 auto modelA = dynamic_cast<CubeModel<TDataType>*>(shapeA);
91 auto modelB = dynamic_cast<CapsuleModel<TDataType>*>(shapeB);
92
93 auto sA = modelA->outCube()->getValue();
94 auto sB = modelB->outCapsule()->getValue();
95 Segment3D seg = sB.centerline();
96 Real radius2 = sB.radius;
97 //CollisionDetection<Real>::request(manifold, sA, sB);
98 CollisionDetection<Real>::request(manifold, sA, seg, 0.01f, radius2);
99 }
100 else if (shapeA->getShapeType() == BasicShapeType::CAPSULE && shapeB->getShapeType() == BasicShapeType::CUBE) //capsule-cube
101 {
102 auto modelA = dynamic_cast<CapsuleModel<TDataType>*>(shapeA);
103 auto modelB = dynamic_cast<CubeModel<TDataType>*>(shapeB);
104
105 auto sA = modelA->outCapsule()->getValue();
106 auto sB = modelB->outCube()->getValue();
107 Segment3D seg = sA.centerline();
108 Real radius1 = sA.radius;
109 //CollisionDetection<Real>::request(manifold, sA, sB);
110 CollisionDetection<Real>::request(manifold, seg, sB, radius1, 0.01f);
111 }
112 else if (shapeA->getShapeType() == BasicShapeType::CUBE && shapeB->getShapeType() == BasicShapeType::SPHERE) //cube-sphere
113 {
114 auto modelA = dynamic_cast<CubeModel<TDataType>*>(shapeA);
115 auto modelB = dynamic_cast<SphereModel<TDataType>*>(shapeB);
116
117 auto sA = modelA->outCube()->getValue();
118 auto sB = modelB->outSphere()->getValue();
119
120 //CollisionDetection<Real>::request(manifold, sA, sB);
121 CollisionDetection<Real>::request(manifold, sA, sB, 0.01f, 0.f);
122 }
123 else if (shapeA->getShapeType() == BasicShapeType::SPHERE && shapeB->getShapeType() == BasicShapeType::CUBE) //sphere-cube
124 {
125 auto modelA = dynamic_cast<SphereModel<TDataType>*>(shapeA);
126 auto modelB = dynamic_cast<CubeModel<TDataType>*>(shapeB);
127
128 auto sA = modelA->outSphere()->getValue();
129 auto sB = modelB->outCube()->getValue();
130
131 //CollisionDetection<Real>::request(manifold, sA, sB);
132 CollisionDetection<Real>::request(manifold, sA, sB, 0.f, 0.01f);
133 }
134 else if (shapeA->getShapeType() == BasicShapeType::SPHERE && shapeB->getShapeType() == BasicShapeType::CAPSULE) //sphere-cube
135 {
136 auto modelA = dynamic_cast<SphereModel<TDataType>*>(shapeA);
137 auto modelB = dynamic_cast<CapsuleModel<TDataType>*>(shapeB);
138
139 auto sA = modelA->outSphere()->getValue();
140 auto sB = modelB->outCapsule()->getValue();
141
142 Segment3D seg = sB.centerline();
143 Real radius2 = sB.radius;
144
145 //CollisionDetection<Real>::request(manifold, sA, sB);
146 CollisionDetection<Real>::request(manifold, sA, seg, 0.f, radius2);
147 }
148 else if (shapeA->getShapeType() == BasicShapeType::CAPSULE && shapeB->getShapeType() == BasicShapeType::SPHERE) //sphere-cube
149 {
150 auto modelA = dynamic_cast<CapsuleModel<TDataType>*>(shapeA);
151 auto modelB = dynamic_cast<SphereModel<TDataType>*>(shapeB);
152
153 auto sA = modelA->outCapsule()->getValue();
154 auto sB = modelB->outSphere()->getValue();
155 Segment3D seg = sA.centerline();
156 Real radius1 = sA.radius;
157
158 //CollisionDetection<Real>::request(manifold, sA, sB);
159 CollisionDetection<Real>::request(manifold, seg, sB, radius1, 0.f);
160 }
161 else
162 std::cout << "Not supported yet" << std::endl;
163
164
165 std::vector<Coord> points;
166
167 std::vector<Coord> vertices;
168 std::vector<TopologyModule::Edge> edges;
169
170
171 uint num = manifold.contactCount;
172 for (uint i = 0; i < num; i++)
173 {
174 points.push_back(manifold.contacts[i].position);
175 vertices.push_back(manifold.contacts[i].position);
176 vertices.push_back(manifold.contacts[i].position + manifold.normal * 0.05);
177 edges.push_back(TopologyModule::Edge(2 * i, 2 * i + 1));
178 }
179
180 auto ptSet = this->stateContacts()->getDataPtr();
181 ptSet->setPoints(points);
182 ptSet->update();
183
184 auto edgeSet = this->stateNormals()->getDataPtr();
185 edgeSet->setPoints(vertices);
186 edgeSet->setEdges(edges);
187 edgeSet->update();
188
189 points.clear();
190 vertices.clear();
191 edges.clear();
192 }
193
194 template<typename TDataType>
196 {
197 return this->getShapeA() != nullptr && this->getShapeB() != nullptr;
198 }
199
201}
#define DEFINE_CLASS(name)
Definition Object.h:140
#define IMPLEMENT_TCLASS(name, T1)
Definition Object.h:103
double Real
Definition Typedef.inl:23
static DYN_FUNC void request(Manifold &m, const Sphere3D &sphereA, const Sphere3D &sphereB, const Real radiusA, const Real radiusB)
std::shared_ptr< GraphicsPipeline > graphicsPipeline()
Definition Node.cpp:320
void setAutoSync(bool con)
Whether the node can be automatically synchronized when its ancestor is updated.
Definition Node.cpp:63
VectorND< PointType, 2 > Edge
This is an implementation of AdditiveCCD based on peridyno.
Definition Array.h:25
@ CAPSULE
Definition BasicShape.h:28
@ SPHERE
Definition BasicShape.h:26
@ CUBE
Definition BasicShape.h:25
unsigned int uint
Definition VkProgram.h:14
TSegment3D< double > Segment3D
Vector< Real, 3 > normal
TContact< Real > contacts[8]