42 M_BOOL laCollider::intersection(
const laLine2 &lnMovement,
const laLine2 &lnCollision,
43 double *pDistance,
laPoint2 *pIntersection)
45 PROFILE_COL(laCollider_intersection);
53 *pIntersection = lnMovement.
at(k);
57 if( (k>=0) && (k<=1) && (k_ln>=0) && (k_ln<=1) )
73 PROFILE_COL(laCollider_response);
74 static double k, k_ln;
78 laPoint2 ptDestination = position + vector;
80 laLine2 lnNormalProjection(ptDestination, ptDestination + normal);
82 lnCollisionCauser.
intersection(lnNormalProjection, &k, &k_ln);
83 laPoint2 ptIntersection = lnCollisionCauser.
at(k);
86 *pModifiedVector = ptIntersection - position;
90 inline void safe_magin(
const laLine2 &lnPositioned, laCollisionScenario *pScenario)
92 PROFILE_COL(laCollider_safe_magin);
94 unsigned nCurrentDomain = 0, i;
96 laRect2 rectBoundingLine, rectBoundingDomain;
99 static double d1, d2, d3, d4, d_min;
102 rectBoundingLine.
add( lnPositioned );
103 rectBoundingLine.
inflate( pScenario->ptModifiedVector );
110 if( rectBoundingLine.intersecting( pDomain->boundingRect() ) )
111 for(i=0; i<pDomain->countLines(); i++)
113 pDomainLine = pDomain->line(i);
117 pt = pDomainLine->
origin + *(pDomain->normal(i));
124 laLine2 lnMagin(lnPositioned.
origin + pScenario->ptModifiedVector, lnPositioned.
end() + pScenario->ptModifiedVector);
125 laLine2 lnMagin2(pDomainLine->
origin - pScenario->ptModifiedVector, pDomainLine->
end() - pScenario->ptModifiedVector);
130 d_min = sqrt( M_MIN(d1, M_MIN(d2, M_MIN(d3, d4))) );
133 pScenario->ptModifiedVector += *(pDomain->normal(i)) * (
M_NORMAL_RESPONSE - d_min );
135 }
while( (pDomain = pDomain->nextDomain()) && (++nCurrentDomain < pScenario->nDomains) );
141 void laCollider::collideLineWithDomain(
const laLine2 &line, laCollisionScenario *pScenario,
142 int nIterations, M_BOOL bResponse)
144 PROFILE_COL(laCollider_collideLineWithDomain);
145 unsigned nCurrentDomain = 0, i, j;
148 static laLine2 *pDomainLine, lnMovement[4];
150 laRect2 rectBoundingLine, rectBoundingDomain;
155 lnPositioned.
origin += pScenario->ptPosition;
157 rectBoundingLine.
add( lnPositioned );
158 rectBoundingLine.
inflate( pScenario->ptVector );
168 lnMovement[0].
build_v( lnPositioned.
origin, pScenario->ptVector );
169 lnMovement[1].
build_v( lnPositioned.
end(), pScenario->ptVector );
173 pScenario->bCollision = M_FALSE;
174 pScenario->ptModifiedVector = pScenario->ptVector;
175 pScenario->pTrap = NULL;
177 double d, dNearestDistance = pScenario->ptVector.lenght_sq() +
M_UNIT;
186 if( rectBoundingLine.intersecting( pDomain->boundingRect() ) )
187 for(i=0; i<pDomain->countLines(); i++)
189 pDomainLine = pDomain->line(i);
193 pt = pDomainLine->
origin + *(pDomain->normal(i));
203 lnMovement[2].
build_v( pDomainLine->
origin, pScenario->ptVector * (-1) );
204 lnMovement[3].
build_v( pDomainLine->
end(), pScenario->ptVector * (-1) );
213 if( intersection(lnMovement[j], (j<2)? *pDomainLine : lnPositioned, &d, &ptIntersection) && (d<dNearestDistance) )
217 dNearestDistance = d;
218 pScenario->lnCollision = *pDomainLine;
221 pScenario->ptCausing = lnMovement[j].
origin;
222 pScenario->ptIntersection = ptIntersection;
226 pScenario->ptCausing = ptIntersection;
227 pScenario->ptIntersection = lnMovement[j].
origin;
230 pScenario->pTrap = pDomain->trap(i);
236 }
while( (pDomain = pDomain->nextDomain()) && (++nCurrentDomain < pScenario->nDomains) );
240 if( dNearestDistance < pScenario->ptVector.lenght_sq() )
242 pScenario->bCollision = M_TRUE;
247 response(pScenario->ptCausing, pScenario->ptVector, pScenario->lnCollision, &(pScenario->ptModifiedVector));
249 pScenario->ptModifiedVector = pScenario->ptIntersection - pScenario->ptCausing;
258 pScenario->ptVector = pScenario->ptModifiedVector;
260 collideLineWithDomain(line, pScenario, nIterations-1);
261 pScenario->ptVector = tv;
262 pScenario->bCollision = M_TRUE;
270 void laCollider::collideLinesWithDomain(
laLine2* arLines,
unsigned nCount, laCollisionScenario *pScenario,
271 int nIterations, M_BOOL bResponse)
273 PROFILE_COL(laCollider_collideLinesWithDomain);
274 laCollisionScenario scenarioClosest = *pScenario;
275 double d, dNearestDistance = pScenario->ptVector.lenght_sq() +
M_UNIT;
280 for(
unsigned i=0; i<nCount; i++)
282 collideLineWithDomain(arLines[i], pScenario, 0, M_FALSE);
283 d = pScenario->ptModifiedVector.lenght_sq();
287 if( d < dNearestDistance ) {
289 dNearestDistance = d;
290 scenarioClosest = *pScenario;
296 if( dNearestDistance < pScenario->ptVector.lenght_sq() )
298 *pScenario = scenarioClosest;
301 response(pScenario->ptCausing, pScenario->ptVector, pScenario->lnCollision, &(pScenario->ptModifiedVector) );
303 pScenario->ptModifiedVector = pScenario->ptIntersection - pScenario->ptCausing;
305 laLine2 ln =
laLine2(arLines[nLine].origin + pScenario->ptPosition, arLines[nLine].
end() + pScenario->ptPosition);
306 safe_magin(ln, pScenario);
312 pScenario->ptVector = pScenario->ptModifiedVector;
314 collideLinesWithDomain(arLines, nCount, pScenario, nIterations-1);
316 pScenario->ptVector = tv;
317 pScenario->bCollision = M_TRUE;
321 else for(
unsigned j=0; j<nIterations; j++)
323 laLine2 ln =
laLine2(arLines[nLine].origin + pScenario->ptPosition, arLines[nLine].
end() + pScenario->ptPosition);
324 safe_magin(ln, pScenario);
330 void laCollider::collidePointWithDomain(laCollisionScenario *pScenario,
int nIterations, M_BOOL bResponse)
332 PROFILE_COL(laCollider_collidePointWithDomain);
333 unsigned nCurrentDomain = 0;
336 static laLine2 *pDomainLine, lnMovement;
340 static double d, dNearestDistance;
342 rect.
add( pScenario->ptPosition );
343 rect.
add( pScenario->ptPosition + pScenario->ptVector );
344 rect.
inflate( pScenario->ptVector );
348 lnMovement.
build_v( pScenario->ptPosition, pScenario->ptVector);
354 if( rect.intersecting(pDomain->boundingRect()) )
355 for(
unsigned i=0; i<pDomain->countLines(); i++)
357 pDomainLine = pDomain->line(i);
361 pt = pDomainLine->
origin + *(pDomain->normal(i));
362 if( pDomainLine->
direction(pt)!=pDomainLine->
direction(pScenario->ptPosition) )
continue;
367 if( intersection(lnMovement, *pDomainLine, &d, &ptIntersection) && (d<dNearestDistance) )
369 dNearestDistance = d;
370 pScenario->lnCollision = *pDomainLine;
371 pScenario->ptIntersection = ptIntersection;
373 pScenario->pTrap = pDomain->trap(i);
376 }
while( (pDomain = pDomain->nextDomain()) && (++nCurrentDomain < pScenario->nDomains) );
380 if( dNearestDistance < lnMovement.
lenght_sq() )
382 pScenario->bCollision = M_TRUE;
386 response(pScenario->ptPosition, pScenario->ptVector, pScenario->lnCollision, &(pScenario->ptModifiedVector));
388 pScenario->ptModifiedVector = pScenario->ptIntersection - pScenario->ptPosition;
409 pScenario->bCollision = M_FALSE;
410 pScenario->ptModifiedVector = pScenario->ptVector;
411 pScenario->pTrap = NULL;
418 void laCollider::collidePointsWithDomain(
laPoint2* arPoint,
unsigned nCount, laCollisionScenario *pScenario,
int nIterations)
420 ASSERT(M_FALSE,
"Not Implelemted");
#define M_NORMAL_RESPONSE
Distance an object "bounces back" form a collision surface.
M_BOOL onSameSide(const laPoint2 &a, const laPoint2 &b) const
Check if two points are on the same side of the line.
double lenght_sq() const
Squared root of segment lenght.
int direction(const laPoint2 &pt) const
#define M_UNIT
Unit of 1 meter.
void build_v(const laPoint2 &a, const laPoint2 &v)
Build a line from an end-point and a vector.
M_BOOL intersection(const laLine2 &ln, double *k, double *k_ln) const
void add(const laPoint2 &p)
Revert to "uninitialized" state.
laPoint2 end() const
Get the end-point.
void inflate(const laPoint2 &inf)
Add vector to the rectangle (e.g. velocity)
void at(double k, laPoint2 *ppt) const
Get point at 1/k lenght (returns in user-specified pointer)
laPoint2 normal(int direction=1) const
Get a perpendicular vector of lenght 1 (normal of the line segment)
2D rectangle (used by the GUI and also as a simple tool for proximity testing)
laPoint2 origin
Point of origin.
double distance_sq(laPoint2 &pt) const
Compute the square of the distance between a point and the line.