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
|
/* This file is part of StepCore library.
Copyright (C) 2007 Vladimir Kuznetsov <ks.vladimir@gmail.com>
StepCore library is free software; you can redistribute it and/or modify
it under the terms of the GNU General Public License as published by
the Free Software Foundation; either version 2 of the License, or
(at your option) any later version.
StepCore library is distributed in the hope that it will be useful,
but WITHOUT ANY WARRANTY; without even the implied warranty of
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
GNU General Public License for more details.
You should have received a copy of the GNU General Public License
along with StepCore; if not, write to the Free Software
Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA
*/
/** \file collisionsolver.h
* \brief CollisionSolver interface
*/
#ifndef STEPCORE_COLLISIONSOLVER_H
#define STEPCORE_COLLISIONSOLVER_H
#include "object.h"
#include "world.h"
#include "vector.h"
#include "solver.h"
#define EIGEN_USE_NEW_STDVECTOR
#include <Eigen/StdVector>
namespace StepCore
{
class BasePolygon;
class Body;
/** \ingroup contacts
* \brief Description of contact between two bodies
*/
struct Contact {
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
enum {
Unknown = 0, /**< Contact state was not (can not) be determined
(if state == Unknown all other fields are not used) */
Separated, /**< Bodies are far away */
Separating, /**< Bodies are contacted but moving apart */
Contacted, /**< Bodies are contacted but resting */
Colliding, /**< Bodies are collising */
Intersected /**< Bodies are interpenetrating */
};
enum {
UnknownType,
PolygonPolygonType,
PolygonDiskType,
PolygonParticleType,
DiskDiskType,
DiskParticleType
};
int type; /**< Contact type (used internally) */
Body* body0; /**< Body0 */
Body* body1; /**< Body1 */
int state; /**< Contact state (maximum of pointsState if pointsCount > 0) */
double distance; /**< Distance between bodies */
Vector2d normal; /**< Contact normal (pointing from body0 to body1) */
Vector2d normalDerivative; /**< Time derivative of contact normal (only if state == Contacted) */
int pointsCount; /**< Count of contact points (either one or two) */
int pointsState[2];/**< Contact point states */
Vector2d points[2]; /**< Contact point coordinated */
double vrel[2]; /**< Relative velocities at contact points */
// Cached values from previous run
// TODO: move it to GJK-specific derived struct
int _w1[2];
};
/** \ingroup contacts
* \brief Collision solver interface
*
* Provides generic interface for collision solvers.
*/
class CollisionSolver : public Object
{
STEPCORE_OBJECT(CollisionSolver)
public:
CollisionSolver(): _toleranceAbs(0.001), _localError(0) {}
virtual ~CollisionSolver() {}
/** Get absolute allowed tolerance */
double toleranceAbs() const { return _toleranceAbs; }
/** Set absolute allowed tolerance */
virtual void setToleranceAbs(double toleranceAbs) { _toleranceAbs = toleranceAbs; }
/** Get error estimation from last step */
double localError() const { return _localError; }
/** Check (and update) state of the contact
* \param contact contact to check (only body0 and body1 fields must be set)
* \return state of the contact (equals to contact->state)
*/
//virtual int checkContact(Contact* contact) = 0;
/** Check and count contacts between several bodies
* \param bodies list of bodies to check
* \param count number of contacts
* \return maximum contact state (i.e. maximum value of Contact::state)
*/
virtual int checkContacts(BodyList& bodies, bool collisions = false, int* count = NULL) = 0;
/** Fill the constraint info structure with the contacts computed by checkContacts()
* \param info ConstraintsInfo structure to fill
*/
virtual void getContactsInfo(ConstraintsInfo& info, bool collisions = false) = 0;
// TODO: add errors
/** Solve the collisions between bodies
*/
virtual int solveCollisions(BodyList& bodies) = 0;
/** Reset internal caches of collision information
* @todo do it automatically by checking the cache
*/
virtual void resetCaches() {}
virtual void bodyAdded(BodyList&, Body*) {}
virtual void bodyRemoved(BodyList&, Body*) {}
public:
enum {
InternalError = Solver::CollisionError
};
protected:
double _toleranceAbs;
//double _toleranceRel;
double _localError;
};
typedef std::vector<Contact, Eigen::aligned_allocator<Contact> >
ContactValueList;
/** \ingroup contacts
* \brief Discrete collision solver using Gilbert-Johnson-Keerthi distance algorithm
*
* Objects are treated as colliding if distance between them is greater than zero
* but smaller than certain small value. If distance is less than zero objects are
* always treated as interpenetrating - this signals World::doEvolve to invalidate
* current time step and try with smaller stepSize until objects are colliding but
* not interpenetrating.
*/
class GJKCollisionSolver : public CollisionSolver
{
STEPCORE_OBJECT(GJKCollisionSolver)
public:
GJKCollisionSolver() : _contactsIsValid(false) {}
// TODO: proper copying of the cache !
GJKCollisionSolver(const GJKCollisionSolver& solver)
: CollisionSolver(solver), _contactsIsValid(false) {}
GJKCollisionSolver& operator=(const GJKCollisionSolver&) {
_contactsIsValid = false; return *this; }
/*
enum {
OK = 0,
CollisionDetected = 4096,
PenetrationDetected = 4097
};*/
int checkContacts(BodyList& bodies, bool collisions = false, int* count = NULL);
void getContactsInfo(ConstraintsInfo& info, bool collisions = false);
//int findClosestPoints(const BasePolygon* polygon1, const BasePolygon* polygon2);
int solveCollisions(BodyList& bodies);
//int solveConstraints(BodyList& bodies);
void resetCaches();
void bodyAdded(BodyList& bodies, Body* body);
void bodyRemoved(BodyList& bodies, Body* body);
protected:
int checkContact(Contact* contact);
int checkPolygonPolygon(Contact* contact);
int solvePolygonPolygon(Contact* contact);
int checkPolygonParticle(Contact* contact);
int solvePolygonParticle(Contact* contact);
int checkPolygonDisk(Contact* contact);
int solvePolygonDisk(Contact* contact);
int checkDiskDisk(Contact* contact);
int solveDiskDisk(Contact* contact);
int checkDiskParticle(Contact* contact);
int solveDiskParticle(Contact* contact);
void addContact(Body* body0, Body* body1);
void checkCache(BodyList& bodies);
protected:
ContactValueList _contacts;
bool _contactsIsValid;
};
} // namespace StepCore
#endif
|
