1 /*
2 Copyright (C) 2005 by Ruben Henner Zilibowitz <rzilibowitz@users.sourceforge.net>
3 Part of the Toy Cars Project http://toycars.sourceforge.net
4
5 This program is free software; you can redistribute it and/or modify
6 it under the terms of the license.
7 This program is distributed in the hope that it will be useful,
8 but WITHOUT ANY WARRANTY.
9
10 See the COPYING file for more details.
11 */
12
13 /*
14 * Created by Ruben on Wed Aug 25 2004.
15 */
16
17 #include "Convex.h"
18
19 // if sin^2 of an angle between two edges is less than this, they are contacting edges.
20 const double kEdgeEdgeThreshold = 0.0001; // 0.0003 is just less than 1 degree
21
22 // function declarations
23 void crop_edges(const Tuple& N, Tuple& P1, Tuple& Q1, Tuple& P2, Tuple& Q2);
24
Convex(const Tuple * array,int size)25 Convex::Convex(const Tuple* array, int size)
26 {
27 for (int n = 0; n < size; n++)
28 push_back(array[n]);
29 }
30
31
32 // check for collisions between two geoms.
collide(const Convex & geomA,const Convex & geomB,list<Contact> & contacts)33 bool Convex::collide(const Convex& geomA, const Convex& geomB, list<Contact>& contacts)
34 {
35 const Convex* geom_touching = &geomA;
36 const Convex* geom_receiving = &geomB;
37
38 double depth;
39 Convex::const_iterator Va, Vb, P;
40 Tuple N;
41
42 double bestdepth;
43 Convex::const_iterator bestVa, bestVb, bestP;
44 Tuple bestN;
45
46 // initialise variables
47 bestdepth = HUGE_VAL;
48
49 for(Va = geomB.begin(), Vb = geomB.last(); Va != geomB.end(); Vb = Va++)
50 {
51 // compute normal direction. (length is not normalised)
52 N = (*Vb - *Va).perp();
53
54 // calculate deepest point of other polygon along this normal
55 P = geomA.deepest_point(N);
56
57 // the penetration depth
58 depth = (*P - *Vb)*N;
59
60 // the edge separates the polygons, they are therefore disjoint
61 if (depth > 0)
62 return false;
63
64 // convert to the square of the normalised depth
65 depth = sqr(depth)/(N*N);
66
67 // is it smallest penetration found?
68 if (depth < bestdepth)
69 {
70 bestVa = Va;
71 bestVb = Vb;
72 bestP = P;
73 bestN = N;
74 bestdepth = depth;
75 }
76 }
77
78 // check geomB against edges of geomA
79 for(Va = geomA.begin(), Vb = geomA.last(); Va != geomA.end(); Vb = Va++)
80 {
81 // compute normal direction. (length is not normalised)
82 N = (*Vb - *Va).perp();
83
84 // calculate deepest point of other polygon along this normal
85 P = geomB.deepest_point(N);
86
87 // the penetration depth
88 depth = (*P - *Vb)*N;
89
90 // the edge separates the polygons, they are therefore disjoint
91 if (depth > 0)
92 return false;
93
94 // convert to the square of the normalised depth
95 depth = sqr(depth)/(N*N);
96
97 // is it smallest penetration found?
98 if (depth < bestdepth)
99 {
100 bestVa = Va;
101 bestVb = Vb;
102 bestP = P;
103 bestN = N;
104 bestdepth = depth;
105 geom_touching = &geomB;
106 geom_receiving = &geomA;
107 }
108 }
109
110 // Normalise
111
112 bestN.Normalise();
113
114 // Determine type of collision (edge/edge or vertex/edge)
115
116 Convex::const_iterator nextP = geom_touching->next(bestP);
117 Convex::const_iterator prevP = geom_touching->prev(bestP);
118
119 Tuple PA = *nextP - *bestP;
120 Tuple BP = *bestP - *prevP;
121
122 if (sqr(PA*bestN)/(PA*PA) < kEdgeEdgeThreshold) // Check for edge/edge collision
123 {
124 Tuple P1 = *nextP;
125 Tuple Q1 = *bestP;
126 Tuple P2 = *bestVa;
127 Tuple Q2 = *bestVb;
128
129 crop_edges(bestN, P1, Q1, P2, Q2);
130
131 contacts.push_back(Contact(geom_touching->parent, geom_receiving->parent, P1, Q2, bestN));
132 contacts.push_back(Contact(geom_touching->parent, geom_receiving->parent, Q1, P2, bestN));
133 }
134 else if (sqr(BP*bestN)/(BP*BP) < kEdgeEdgeThreshold) // Check for edge/edge collision
135 {
136 Tuple P1 = *bestP;
137 Tuple Q1 = *prevP;
138 Tuple P2 = *bestVa;
139 Tuple Q2 = *bestVb;
140
141 crop_edges(bestN, P1, Q1, P2, Q2);
142
143 contacts.push_back(Contact(geom_touching->parent, geom_receiving->parent, P1, Q2, bestN));
144 contacts.push_back(Contact(geom_touching->parent, geom_receiving->parent, Q1, P2, bestN));
145 }
146 else // it is a vertex/edge collision
147 {
148 Tuple edge = *bestVa - *bestVb;
149 double lambda = ((*bestP - *bestVb)*edge)/(edge*edge);
150 Tuple Q = (*bestVb) + lambda*edge;
151
152 contacts.push_back(Contact(geom_touching->parent, geom_receiving->parent, *bestP, Q, bestN));
153 }
154
155 return true;
156 }
157
158 // find the vertex which lies deepest against the direction given as N.
deepest_point(const Tuple & N) const159 Convex::const_iterator Convex::deepest_point(const Tuple& N) const
160 {
161 double d;
162 double spanmin = HUGE_VAL;
163
164 // compute the deepest penetrating point
165
166 const_iterator minV;
167
168 for(const_iterator V = begin(); V != end(); V++)
169 {
170 d = (*V)*N;
171
172 if (d < spanmin)
173 {
174 spanmin = d;
175 minV = V;
176 }
177 }
178
179 return minV;
180 }
181
182 // crop_edges takes two line segments given by (P1..Q1) and (P2..Q2) and crops them
183 // down so that (P1..Q2) is in same direction as N and (Q1..P2) is in same direction as N.
184 // it is assumed that they are oriented oppositely and are roughly at same angle.
crop_edges(const Tuple & N,Tuple & P1,Tuple & Q1,Tuple & P2,Tuple & Q2)185 void crop_edges(const Tuple& N, Tuple& P1, Tuple& Q1, Tuple& P2, Tuple& Q2)
186 {
187 double lambda;
188
189 Tuple newP1 = P1;
190 Tuple newQ2 = Q2;
191
192 lambda = (N^(Q2 - Q1))/(N^(P1 - Q1));
193 if (lambda < 1)
194 {
195 newP1 = lambda*(P1 - Q1) + Q1;
196 }
197 else
198 {
199 lambda = (N^(P1 - Q2))/(N^(P2 - Q2));
200 newQ2 = lambda*(P2 - Q2) + Q2;
201 }
202
203 lambda = (N^(P2 - Q1))/(N^(P1 - Q1));
204 if (lambda > 0)
205 {
206 Q1 = lambda*(P1 - Q1) + Q1;
207 }
208 else
209 {
210 lambda = (N^(Q1 - Q2))/(N^(P2 - Q2));
211 P2 = lambda*(P2 - Q2) + Q2;
212 }
213
214 P1 = newP1;
215 Q2 = newQ2;
216 }
217