aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/src/others/irrlicht-1.8.1/source/Irrlicht/CSceneCollisionManager.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/others/irrlicht-1.8.1/source/Irrlicht/CSceneCollisionManager.cpp')
-rw-r--r--src/others/irrlicht-1.8.1/source/Irrlicht/CSceneCollisionManager.cpp960
1 files changed, 960 insertions, 0 deletions
diff --git a/src/others/irrlicht-1.8.1/source/Irrlicht/CSceneCollisionManager.cpp b/src/others/irrlicht-1.8.1/source/Irrlicht/CSceneCollisionManager.cpp
new file mode 100644
index 0000000..cda4133
--- /dev/null
+++ b/src/others/irrlicht-1.8.1/source/Irrlicht/CSceneCollisionManager.cpp
@@ -0,0 +1,960 @@
1// Copyright (C) 2002-2012 Nikolaus Gebhardt
2// This file is part of the "Irrlicht Engine".
3// For conditions of distribution and use, see copyright notice in irrlicht.h
4
5#include "CSceneCollisionManager.h"
6#include "ISceneNode.h"
7#include "ICameraSceneNode.h"
8#include "ITriangleSelector.h"
9#include "SViewFrustum.h"
10
11#include "os.h"
12#include "irrMath.h"
13
14namespace irr
15{
16namespace scene
17{
18
19//! constructor
20CSceneCollisionManager::CSceneCollisionManager(ISceneManager* smanager, video::IVideoDriver* driver)
21: SceneManager(smanager), Driver(driver)
22{
23 #ifdef _DEBUG
24 setDebugName("CSceneCollisionManager");
25 #endif
26
27 if (Driver)
28 Driver->grab();
29}
30
31
32//! destructor
33CSceneCollisionManager::~CSceneCollisionManager()
34{
35 if (Driver)
36 Driver->drop();
37}
38
39
40//! Returns the scene node, which is currently visible at the given
41//! screen coordinates, viewed from the currently active camera.
42ISceneNode* CSceneCollisionManager::getSceneNodeFromScreenCoordinatesBB(
43 const core::position2d<s32>& pos, s32 idBitMask, bool noDebugObjects, scene::ISceneNode* root)
44{
45 const core::line3d<f32> ln = getRayFromScreenCoordinates(pos, 0);
46
47 if ( ln.start == ln.end )
48 return 0;
49
50 return getSceneNodeFromRayBB(ln, idBitMask, noDebugObjects, root);
51}
52
53
54//! Returns the nearest scene node which collides with a 3d ray and
55//! which id matches a bitmask.
56ISceneNode* CSceneCollisionManager::getSceneNodeFromRayBB(
57 const core::line3d<f32>& ray,
58 s32 idBitMask, bool noDebugObjects, scene::ISceneNode* root)
59{
60 ISceneNode* best = 0;
61 f32 dist = FLT_MAX;
62
63 core::line3d<f32> truncatableRay(ray);
64
65 getPickedNodeBB((root==0)?SceneManager->getRootSceneNode():root, truncatableRay,
66 idBitMask, noDebugObjects, dist, best);
67
68 return best;
69}
70
71
72//! recursive method for going through all scene nodes
73void CSceneCollisionManager::getPickedNodeBB(ISceneNode* root,
74 core::line3df& ray, s32 bits, bool noDebugObjects,
75 f32& outbestdistance, ISceneNode*& outbestnode)
76{
77 const ISceneNodeList& children = root->getChildren();
78 const core::vector3df rayVector = ray.getVector().normalize();
79
80 ISceneNodeList::ConstIterator it = children.begin();
81 for (; it != children.end(); ++it)
82 {
83 ISceneNode* current = *it;
84
85 if (current->isVisible())
86 {
87 if((noDebugObjects ? !current->isDebugObject() : true) &&
88 (bits==0 || (bits != 0 && (current->getID() & bits))))
89 {
90 // get world to object space transform
91 core::matrix4 worldToObject;
92 if (!current->getAbsoluteTransformation().getInverse(worldToObject))
93 continue;
94
95 // transform vector from world space to object space
96 core::line3df objectRay(ray);
97 worldToObject.transformVect(objectRay.start);
98 worldToObject.transformVect(objectRay.end);
99
100 const core::aabbox3df & objectBox = current->getBoundingBox();
101
102 // Do the initial intersection test in object space, since the
103 // object space box test is more accurate.
104 if(objectBox.isPointInside(objectRay.start))
105 {
106 // use fast bbox intersection to find distance to hitpoint
107 // algorithm from Kay et al., code from gamedev.net
108 const core::vector3df dir = (objectRay.end-objectRay.start).normalize();
109 const core::vector3df minDist = (objectBox.MinEdge - objectRay.start)/dir;
110 const core::vector3df maxDist = (objectBox.MaxEdge - objectRay.start)/dir;
111 const core::vector3df realMin(core::min_(minDist.X, maxDist.X),core::min_(minDist.Y, maxDist.Y),core::min_(minDist.Z, maxDist.Z));
112 const core::vector3df realMax(core::max_(minDist.X, maxDist.X),core::max_(minDist.Y, maxDist.Y),core::max_(minDist.Z, maxDist.Z));
113
114 const f32 minmax = core::min_(realMax.X, realMax.Y, realMax.Z);
115 // nearest distance to intersection
116 const f32 maxmin = core::max_(realMin.X, realMin.Y, realMin.Z);
117
118 const f32 toIntersectionSq = (maxmin>0?maxmin*maxmin:minmax*minmax);
119 if (toIntersectionSq < outbestdistance)
120 {
121 outbestdistance = toIntersectionSq;
122 outbestnode = current;
123
124 // And we can truncate the ray to stop us hitting further nodes.
125 ray.end = ray.start + (rayVector * sqrtf(toIntersectionSq));
126 }
127 }
128 else
129 if (objectBox.intersectsWithLine(objectRay))
130 {
131 // Now transform into world space, since we need to use world space
132 // scales and distances.
133 core::aabbox3df worldBox(objectBox);
134 current->getAbsoluteTransformation().transformBox(worldBox);
135
136 core::vector3df edges[8];
137 worldBox.getEdges(edges);
138
139 /* We need to check against each of 6 faces, composed of these corners:
140 /3--------/7
141 / | / |
142 / | / |
143 1---------5 |
144 | 2- - -| -6
145 | / | /
146 |/ | /
147 0---------4/
148
149 Note that we define them as opposite pairs of faces.
150 */
151 static const s32 faceEdges[6][3] =
152 {
153 { 0, 1, 5 }, // Front
154 { 6, 7, 3 }, // Back
155 { 2, 3, 1 }, // Left
156 { 4, 5, 7 }, // Right
157 { 1, 3, 7 }, // Top
158 { 2, 0, 4 } // Bottom
159 };
160
161 core::vector3df intersection;
162 core::plane3df facePlane;
163 f32 bestDistToBoxBorder = FLT_MAX;
164 f32 bestToIntersectionSq = FLT_MAX;
165
166 for(s32 face = 0; face < 6; ++face)
167 {
168 facePlane.setPlane(edges[faceEdges[face][0]],
169 edges[faceEdges[face][1]],
170 edges[faceEdges[face][2]]);
171
172 // Only consider lines that might be entering through this face, since we
173 // already know that the start point is outside the box.
174 if(facePlane.classifyPointRelation(ray.start) != core::ISREL3D_FRONT)
175 continue;
176
177 // Don't bother using a limited ray, since we already know that it should be long
178 // enough to intersect with the box.
179 if(facePlane.getIntersectionWithLine(ray.start, rayVector, intersection))
180 {
181 const f32 toIntersectionSq = ray.start.getDistanceFromSQ(intersection);
182 if(toIntersectionSq < outbestdistance)
183 {
184 // We have to check that the intersection with this plane is actually
185 // on the box, so need to go back to object space again.
186 worldToObject.transformVect(intersection);
187
188 // find the closest point on the box borders. Have to do this as exact checks will fail due to floating point problems.
189 f32 distToBorder = core::max_ ( core::min_ (core::abs_(objectBox.MinEdge.X-intersection.X), core::abs_(objectBox.MaxEdge.X-intersection.X)),
190 core::min_ (core::abs_(objectBox.MinEdge.Y-intersection.Y), core::abs_(objectBox.MaxEdge.Y-intersection.Y)),
191 core::min_ (core::abs_(objectBox.MinEdge.Z-intersection.Z), core::abs_(objectBox.MaxEdge.Z-intersection.Z)) );
192 if ( distToBorder < bestDistToBoxBorder )
193 {
194 bestDistToBoxBorder = distToBorder;
195 bestToIntersectionSq = toIntersectionSq;
196 }
197 }
198 }
199
200 // If the ray could be entering through the first face of a pair, then it can't
201 // also be entering through the opposite face, and so we can skip that face.
202 if (!(face & 0x01))
203 ++face;
204 }
205
206 if ( bestDistToBoxBorder < FLT_MAX )
207 {
208 outbestdistance = bestToIntersectionSq;
209 outbestnode = current;
210
211 // If we got a hit, we can now truncate the ray to stop us hitting further nodes.
212 ray.end = ray.start + (rayVector * sqrtf(outbestdistance));
213 }
214 }
215 }
216
217 // Only check the children if this node is visible.
218 getPickedNodeBB(current, ray, bits, noDebugObjects, outbestdistance, outbestnode);
219 }
220 }
221}
222
223
224ISceneNode* CSceneCollisionManager::getSceneNodeAndCollisionPointFromRay(
225 core::line3df ray,
226 core::vector3df & outCollisionPoint,
227 core::triangle3df & outTriangle,
228 s32 idBitMask,
229 ISceneNode * collisionRootNode,
230 bool noDebugObjects)
231{
232 ISceneNode* bestNode = 0;
233 f32 bestDistanceSquared = FLT_MAX;
234
235 if(0 == collisionRootNode)
236 collisionRootNode = SceneManager->getRootSceneNode();
237
238 // We don't try to do anything too clever, like sorting the candidate
239 // nodes by distance to bounding-box. In the example below, we could do the
240 // triangle collision check with node A first, but we'd have to check node B
241 // anyway, as the actual collision point could be (and is) closer than the
242 // collision point in node A.
243 //
244 // ray end
245 // |
246 // AAAAAAAAAA
247 // A |
248 // A | B
249 // A | B
250 // A BBBBB
251 // A |
252 // A |
253 // |
254 // |
255 // ray start
256 //
257 // We therefore have to do a full BB and triangle collision on every scene
258 // node in order to find the nearest collision point, so sorting them by
259 // bounding box would be pointless.
260
261 getPickedNodeFromBBAndSelector(collisionRootNode, ray, idBitMask,
262 noDebugObjects, bestDistanceSquared, bestNode,
263 outCollisionPoint, outTriangle);
264 return bestNode;
265}
266
267
268void CSceneCollisionManager::getPickedNodeFromBBAndSelector(
269 ISceneNode * root,
270 core::line3df & ray,
271 s32 bits,
272 bool noDebugObjects,
273 f32 & outBestDistanceSquared,
274 ISceneNode * & outBestNode,
275 core::vector3df & outBestCollisionPoint,
276 core::triangle3df & outBestTriangle)
277{
278 const ISceneNodeList& children = root->getChildren();
279
280 ISceneNodeList::ConstIterator it = children.begin();
281 for (; it != children.end(); ++it)
282 {
283 ISceneNode* current = *it;
284 ITriangleSelector * selector = current->getTriangleSelector();
285
286 if (selector && current->isVisible() &&
287 (noDebugObjects ? !current->isDebugObject() : true) &&
288 (bits==0 || (bits != 0 && (current->getID() & bits))))
289 {
290 // get world to object space transform
291 core::matrix4 mat;
292 if (!current->getAbsoluteTransformation().getInverse(mat))
293 continue;
294
295 // transform vector from world space to object space
296 core::line3df line(ray);
297 mat.transformVect(line.start);
298 mat.transformVect(line.end);
299
300 const core::aabbox3df& box = current->getBoundingBox();
301
302 core::vector3df candidateCollisionPoint;
303 core::triangle3df candidateTriangle;
304
305 // do intersection test in object space
306 ISceneNode * hitNode = 0;
307 if (box.intersectsWithLine(line) &&
308 getCollisionPoint(ray, selector, candidateCollisionPoint, candidateTriangle, hitNode))
309 {
310 const f32 distanceSquared = (candidateCollisionPoint - ray.start).getLengthSQ();
311
312 if(distanceSquared < outBestDistanceSquared)
313 {
314 outBestDistanceSquared = distanceSquared;
315 outBestNode = current;
316 outBestCollisionPoint = candidateCollisionPoint;
317 outBestTriangle = candidateTriangle;
318 const core::vector3df rayVector = ray.getVector().normalize();
319 ray.end = ray.start + (rayVector * sqrtf(distanceSquared));
320 }
321 }
322 }
323
324 getPickedNodeFromBBAndSelector(current, ray, bits, noDebugObjects,
325 outBestDistanceSquared, outBestNode,
326 outBestCollisionPoint, outBestTriangle);
327 }
328}
329
330
331//! Returns the scene node, at which the overgiven camera is looking at and
332//! which id matches the bitmask.
333ISceneNode* CSceneCollisionManager::getSceneNodeFromCameraBB(
334 ICameraSceneNode* camera, s32 idBitMask, bool noDebugObjects)
335{
336 if (!camera)
337 return 0;
338
339 const core::vector3df start = camera->getAbsolutePosition();
340 core::vector3df end = camera->getTarget();
341
342 end = start + ((end - start).normalize() * camera->getFarValue());
343
344 return getSceneNodeFromRayBB(core::line3d<f32>(start, end), idBitMask, noDebugObjects);
345}
346
347
348//! Finds the collision point of a line and lots of triangles, if there is one.
349bool CSceneCollisionManager::getCollisionPoint(const core::line3d<f32>& ray,
350 ITriangleSelector* selector, core::vector3df& outIntersection,
351 core::triangle3df& outTriangle,
352 ISceneNode*& outNode)
353{
354 if (!selector)
355 {
356 _IRR_IMPLEMENT_MANAGED_MARSHALLING_BUGFIX;
357 return false;
358 }
359
360 s32 totalcnt = selector->getTriangleCount();
361 if ( totalcnt <= 0 )
362 return false;
363
364 Triangles.set_used(totalcnt);
365
366 s32 cnt = 0;
367 selector->getTriangles(Triangles.pointer(), totalcnt, cnt, ray);
368
369 const core::vector3df linevect = ray.getVector().normalize();
370 core::vector3df intersection;
371 f32 nearest = FLT_MAX;
372 bool found = false;
373 const f32 raylength = ray.getLengthSQ();
374
375 const f32 minX = core::min_(ray.start.X, ray.end.X);
376 const f32 maxX = core::max_(ray.start.X, ray.end.X);
377 const f32 minY = core::min_(ray.start.Y, ray.end.Y);
378 const f32 maxY = core::max_(ray.start.Y, ray.end.Y);
379 const f32 minZ = core::min_(ray.start.Z, ray.end.Z);
380 const f32 maxZ = core::max_(ray.start.Z, ray.end.Z);
381
382 for (s32 i=0; i<cnt; ++i)
383 {
384 const core::triangle3df & triangle = Triangles[i];
385
386 if(minX > triangle.pointA.X && minX > triangle.pointB.X && minX > triangle.pointC.X)
387 continue;
388 if(maxX < triangle.pointA.X && maxX < triangle.pointB.X && maxX < triangle.pointC.X)
389 continue;
390 if(minY > triangle.pointA.Y && minY > triangle.pointB.Y && minY > triangle.pointC.Y)
391 continue;
392 if(maxY < triangle.pointA.Y && maxY < triangle.pointB.Y && maxY < triangle.pointC.Y)
393 continue;
394 if(minZ > triangle.pointA.Z && minZ > triangle.pointB.Z && minZ > triangle.pointC.Z)
395 continue;
396 if(maxZ < triangle.pointA.Z && maxZ < triangle.pointB.Z && maxZ < triangle.pointC.Z)
397 continue;
398
399 if (triangle.getIntersectionWithLine(ray.start, linevect, intersection))
400 {
401 const f32 tmp = intersection.getDistanceFromSQ(ray.start);
402 const f32 tmp2 = intersection.getDistanceFromSQ(ray.end);
403
404 if (tmp < raylength && tmp2 < raylength && tmp < nearest)
405 {
406 nearest = tmp;
407 outTriangle = triangle;
408 outIntersection = intersection;
409 outNode = selector->getSceneNodeForTriangle(i);
410 found = true;
411 }
412 }
413 }
414
415 _IRR_IMPLEMENT_MANAGED_MARSHALLING_BUGFIX;
416 return found;
417}
418
419
420//! Collides a moving ellipsoid with a 3d world with gravity and returns
421//! the resulting new position of the ellipsoid.
422core::vector3df CSceneCollisionManager::getCollisionResultPosition(
423 ITriangleSelector* selector,
424 const core::vector3df &position, const core::vector3df& radius,
425 const core::vector3df& direction,
426 core::triangle3df& triout,
427 core::vector3df& hitPosition,
428 bool& outFalling,
429 ISceneNode*& outNode,
430 f32 slidingSpeed,
431 const core::vector3df& gravity)
432{
433 return collideEllipsoidWithWorld(selector, position,
434 radius, direction, slidingSpeed, gravity, triout, hitPosition, outFalling, outNode);
435}
436
437
438bool CSceneCollisionManager::testTriangleIntersection(SCollisionData* colData,
439 const core::triangle3df& triangle)
440{
441 const core::plane3d<f32> trianglePlane = triangle.getPlane();
442
443 // only check front facing polygons
444 if ( !trianglePlane.isFrontFacing(colData->normalizedVelocity) )
445 return false;
446
447 // get interval of plane intersection
448
449 f32 t1, t0;
450 bool embeddedInPlane = false;
451
452 // calculate signed distance from sphere position to triangle plane
453 f32 signedDistToTrianglePlane = trianglePlane.getDistanceTo(
454 colData->basePoint);
455
456 f32 normalDotVelocity =
457 trianglePlane.Normal.dotProduct(colData->velocity);
458
459 if ( core::iszero ( normalDotVelocity ) )
460 {
461 // sphere is traveling parallel to plane
462
463 if (fabs(signedDistToTrianglePlane) >= 1.0f)
464 return false; // no collision possible
465 else
466 {
467 // sphere is embedded in plane
468 embeddedInPlane = true;
469 t0 = 0.0;
470 t1 = 1.0;
471 }
472 }
473 else
474 {
475 normalDotVelocity = core::reciprocal ( normalDotVelocity );
476
477 // N.D is not 0. Calculate intersection interval
478 t0 = (-1.f - signedDistToTrianglePlane) * normalDotVelocity;
479 t1 = (1.f - signedDistToTrianglePlane) * normalDotVelocity;
480
481 // Swap so t0 < t1
482 if (t0 > t1) { f32 tmp = t1; t1 = t0; t0 = tmp; }
483
484 // check if at least one value is within the range
485 if (t0 > 1.0f || t1 < 0.0f)
486 return false; // both t values are outside 1 and 0, no collision possible
487
488 // clamp to 0 and 1
489 t0 = core::clamp ( t0, 0.f, 1.f );
490 t1 = core::clamp ( t1, 0.f, 1.f );
491 }
492
493 // at this point we have t0 and t1, if there is any intersection, it
494 // is between this interval
495 core::vector3df collisionPoint;
496 bool foundCollision = false;
497 f32 t = 1.0f;
498
499 // first check the easy case: Collision within the triangle;
500 // if this happens, it must be at t0 and this is when the sphere
501 // rests on the front side of the triangle plane. This can only happen
502 // if the sphere is not embedded in the triangle plane.
503
504 if (!embeddedInPlane)
505 {
506 core::vector3df planeIntersectionPoint =
507 (colData->basePoint - trianglePlane.Normal)
508 + (colData->velocity * t0);
509
510 if (triangle.isPointInside(planeIntersectionPoint))
511 {
512 foundCollision = true;
513 t = t0;
514 collisionPoint = planeIntersectionPoint;
515 }
516 }
517
518 // if we havent found a collision already we will have to sweep
519 // the sphere against points and edges of the triangle. Note: A
520 // collision inside the triangle will always happen before a
521 // vertex or edge collision.
522
523 if (!foundCollision)
524 {
525 core::vector3df velocity = colData->velocity;
526 core::vector3df base = colData->basePoint;
527
528 f32 velocitySqaredLength = velocity.getLengthSQ();
529 f32 a,b,c;
530 f32 newT;
531
532 // for each edge or vertex a quadratic equation has to be solved:
533 // a*t^2 + b*t + c = 0. We calculate a,b, and c for each test.
534
535 // check against points
536 a = velocitySqaredLength;
537
538 // p1
539 b = 2.0f * (velocity.dotProduct(base - triangle.pointA));
540 c = (triangle.pointA-base).getLengthSQ() - 1.f;
541 if (getLowestRoot(a,b,c,t, &newT))
542 {
543 t = newT;
544 foundCollision = true;
545 collisionPoint = triangle.pointA;
546 }
547
548 // p2
549 if (!foundCollision)
550 {
551 b = 2.0f * (velocity.dotProduct(base - triangle.pointB));
552 c = (triangle.pointB-base).getLengthSQ() - 1.f;
553 if (getLowestRoot(a,b,c,t, &newT))
554 {
555 t = newT;
556 foundCollision = true;
557 collisionPoint = triangle.pointB;
558 }
559 }
560
561 // p3
562 if (!foundCollision)
563 {
564 b = 2.0f * (velocity.dotProduct(base - triangle.pointC));
565 c = (triangle.pointC-base).getLengthSQ() - 1.f;
566 if (getLowestRoot(a,b,c,t, &newT))
567 {
568 t = newT;
569 foundCollision = true;
570 collisionPoint = triangle.pointC;
571 }
572 }
573
574 // check against edges:
575
576 // p1 --- p2
577 core::vector3df edge = triangle.pointB - triangle.pointA;
578 core::vector3df baseToVertex = triangle.pointA - base;
579 f32 edgeSqaredLength = edge.getLengthSQ();
580 f32 edgeDotVelocity = edge.dotProduct(velocity);
581 f32 edgeDotBaseToVertex = edge.dotProduct(baseToVertex);
582
583 // calculate parameters for equation
584 a = edgeSqaredLength* -velocitySqaredLength +
585 edgeDotVelocity*edgeDotVelocity;
586 b = edgeSqaredLength* (2.f *velocity.dotProduct(baseToVertex)) -
587 2.0f*edgeDotVelocity*edgeDotBaseToVertex;
588 c = edgeSqaredLength* (1.f -baseToVertex.getLengthSQ()) +
589 edgeDotBaseToVertex*edgeDotBaseToVertex;
590
591 // does the swept sphere collide against infinite edge?
592 if (getLowestRoot(a,b,c,t,&newT))
593 {
594 f32 f = (edgeDotVelocity*newT - edgeDotBaseToVertex) / edgeSqaredLength;
595 if (f >=0.0f && f <= 1.0f)
596 {
597 // intersection took place within segment
598 t = newT;
599 foundCollision = true;
600 collisionPoint = triangle.pointA + (edge*f);
601 }
602 }
603
604 // p2 --- p3
605 edge = triangle.pointC-triangle.pointB;
606 baseToVertex = triangle.pointB - base;
607 edgeSqaredLength = edge.getLengthSQ();
608 edgeDotVelocity = edge.dotProduct(velocity);
609 edgeDotBaseToVertex = edge.dotProduct(baseToVertex);
610
611 // calculate parameters for equation
612 a = edgeSqaredLength* -velocitySqaredLength +
613 edgeDotVelocity*edgeDotVelocity;
614 b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) -
615 2.0f*edgeDotVelocity*edgeDotBaseToVertex;
616 c = edgeSqaredLength* (1-baseToVertex.getLengthSQ()) +
617 edgeDotBaseToVertex*edgeDotBaseToVertex;
618
619 // does the swept sphere collide against infinite edge?
620 if (getLowestRoot(a,b,c,t,&newT))
621 {
622 f32 f = (edgeDotVelocity*newT-edgeDotBaseToVertex) /
623 edgeSqaredLength;
624 if (f >=0.0f && f <= 1.0f)
625 {
626 // intersection took place within segment
627 t = newT;
628 foundCollision = true;
629 collisionPoint = triangle.pointB + (edge*f);
630 }
631 }
632
633
634 // p3 --- p1
635 edge = triangle.pointA-triangle.pointC;
636 baseToVertex = triangle.pointC - base;
637 edgeSqaredLength = edge.getLengthSQ();
638 edgeDotVelocity = edge.dotProduct(velocity);
639 edgeDotBaseToVertex = edge.dotProduct(baseToVertex);
640
641 // calculate parameters for equation
642 a = edgeSqaredLength* -velocitySqaredLength +
643 edgeDotVelocity*edgeDotVelocity;
644 b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) -
645 2.0f*edgeDotVelocity*edgeDotBaseToVertex;
646 c = edgeSqaredLength* (1-baseToVertex.getLengthSQ()) +
647 edgeDotBaseToVertex*edgeDotBaseToVertex;
648
649 // does the swept sphere collide against infinite edge?
650 if (getLowestRoot(a,b,c,t,&newT))
651 {
652 f32 f = (edgeDotVelocity*newT-edgeDotBaseToVertex) /
653 edgeSqaredLength;
654 if (f >=0.0f && f <= 1.0f)
655 {
656 // intersection took place within segment
657 t = newT;
658 foundCollision = true;
659 collisionPoint = triangle.pointC + (edge*f);
660 }
661 }
662 }// end no collision found
663
664 // set result:
665 if (foundCollision)
666 {
667 // distance to collision is t
668 f32 distToCollision = t*colData->velocity.getLength();
669
670 // does this triangle qualify for closest hit?
671 if (!colData->foundCollision ||
672 distToCollision < colData->nearestDistance)
673 {
674 colData->nearestDistance = distToCollision;
675 colData->intersectionPoint = collisionPoint;
676 colData->foundCollision = true;
677 colData->intersectionTriangle = triangle;
678 ++colData->triangleHits;
679 return true;
680 }
681 }// end found collision
682
683 return false;
684}
685
686
687//! Collides a moving ellipsoid with a 3d world with gravity and returns
688//! the resulting new position of the ellipsoid.
689core::vector3df CSceneCollisionManager::collideEllipsoidWithWorld(
690 ITriangleSelector* selector, const core::vector3df &position,
691 const core::vector3df& radius, const core::vector3df& velocity,
692 f32 slidingSpeed,
693 const core::vector3df& gravity,
694 core::triangle3df& triout,
695 core::vector3df& hitPosition,
696 bool& outFalling,
697 ISceneNode*& outNode)
698{
699 if (!selector || radius.X == 0.0f || radius.Y == 0.0f || radius.Z == 0.0f)
700 return position;
701
702 // This code is based on the paper "Improved Collision detection and Response"
703 // by Kasper Fauerby, but some parts are modified.
704
705 SCollisionData colData;
706 colData.R3Position = position;
707 colData.R3Velocity = velocity;
708 colData.eRadius = radius;
709 colData.nearestDistance = FLT_MAX;
710 colData.selector = selector;
711 colData.slidingSpeed = slidingSpeed;
712 colData.triangleHits = 0;
713 colData.triangleIndex = -1;
714
715 core::vector3df eSpacePosition = colData.R3Position / colData.eRadius;
716 core::vector3df eSpaceVelocity = colData.R3Velocity / colData.eRadius;
717
718 // iterate until we have our final position
719
720 core::vector3df finalPos = collideWithWorld(
721 0, colData, eSpacePosition, eSpaceVelocity);
722
723 outFalling = false;
724
725 // add gravity
726
727 if (gravity != core::vector3df(0,0,0))
728 {
729 colData.R3Position = finalPos * colData.eRadius;
730 colData.R3Velocity = gravity;
731 colData.triangleHits = 0;
732
733 eSpaceVelocity = gravity/colData.eRadius;
734
735 finalPos = collideWithWorld(0, colData,
736 finalPos, eSpaceVelocity);
737
738 outFalling = (colData.triangleHits == 0);
739 }
740
741 if (colData.triangleHits)
742 {
743 triout = colData.intersectionTriangle;
744 triout.pointA *= colData.eRadius;
745 triout.pointB *= colData.eRadius;
746 triout.pointC *= colData.eRadius;
747 outNode = selector->getSceneNodeForTriangle(colData.triangleIndex);
748 }
749
750 finalPos *= colData.eRadius;
751 hitPosition = colData.intersectionPoint * colData.eRadius;
752 return finalPos;
753}
754
755
756core::vector3df CSceneCollisionManager::collideWithWorld(s32 recursionDepth,
757 SCollisionData &colData, core::vector3df pos, core::vector3df vel)
758{
759 f32 veryCloseDistance = colData.slidingSpeed;
760
761 if (recursionDepth > 5)
762 return pos;
763
764 colData.velocity = vel;
765 colData.normalizedVelocity = vel;
766 colData.normalizedVelocity.normalize();
767 colData.basePoint = pos;
768 colData.foundCollision = false;
769 colData.nearestDistance = FLT_MAX;
770
771 //------------------ collide with world
772
773 // get all triangles with which we might collide
774 core::aabbox3d<f32> box(colData.R3Position);
775 box.addInternalPoint(colData.R3Position + colData.R3Velocity);
776 box.MinEdge -= colData.eRadius;
777 box.MaxEdge += colData.eRadius;
778
779 s32 totalTriangleCnt = colData.selector->getTriangleCount();
780 Triangles.set_used(totalTriangleCnt);
781
782 core::matrix4 scaleMatrix;
783 scaleMatrix.setScale(
784 core::vector3df(1.0f / colData.eRadius.X,
785 1.0f / colData.eRadius.Y,
786 1.0f / colData.eRadius.Z));
787
788 s32 triangleCnt = 0;
789 colData.selector->getTriangles(Triangles.pointer(), totalTriangleCnt, triangleCnt, box, &scaleMatrix);
790
791 for (s32 i=0; i<triangleCnt; ++i)
792 if(testTriangleIntersection(&colData, Triangles[i]))
793 colData.triangleIndex = i;
794
795 //---------------- end collide with world
796
797 if (!colData.foundCollision)
798 return pos + vel;
799
800 // original destination point
801 const core::vector3df destinationPoint = pos + vel;
802 core::vector3df newBasePoint = pos;
803
804 // only update if we are not already very close
805 // and if so only move very close to intersection, not to the
806 // exact point
807 if (colData.nearestDistance >= veryCloseDistance)
808 {
809 core::vector3df v = vel;
810 v.setLength( colData.nearestDistance - veryCloseDistance );
811 newBasePoint = colData.basePoint + v;
812
813 v.normalize();
814 colData.intersectionPoint -= (v * veryCloseDistance);
815 }
816
817 // calculate sliding plane
818
819 const core::vector3df slidePlaneOrigin = colData.intersectionPoint;
820 const core::vector3df slidePlaneNormal = (newBasePoint - colData.intersectionPoint).normalize();
821 core::plane3d<f32> slidingPlane(slidePlaneOrigin, slidePlaneNormal);
822
823 core::vector3df newDestinationPoint =
824 destinationPoint -
825 (slidePlaneNormal * slidingPlane.getDistanceTo(destinationPoint));
826
827 // generate slide vector
828
829 const core::vector3df newVelocityVector = newDestinationPoint -
830 colData.intersectionPoint;
831
832 if (newVelocityVector.getLength() < veryCloseDistance)
833 return newBasePoint;
834
835 return collideWithWorld(recursionDepth+1, colData,
836 newBasePoint, newVelocityVector);
837}
838
839
840//! Returns a 3d ray which would go through the 2d screen coodinates.
841core::line3d<f32> CSceneCollisionManager::getRayFromScreenCoordinates(
842 const core::position2d<s32> & pos, ICameraSceneNode* camera)
843{
844 core::line3d<f32> ln(0,0,0,0,0,0);
845
846 if (!SceneManager)
847 return ln;
848
849 if (!camera)
850 camera = SceneManager->getActiveCamera();
851
852 if (!camera)
853 return ln;
854
855 const scene::SViewFrustum* f = camera->getViewFrustum();
856
857 core::vector3df farLeftUp = f->getFarLeftUp();
858 core::vector3df lefttoright = f->getFarRightUp() - farLeftUp;
859 core::vector3df uptodown = f->getFarLeftDown() - farLeftUp;
860
861 const core::rect<s32>& viewPort = Driver->getViewPort();
862 core::dimension2d<u32> screenSize(viewPort.getWidth(), viewPort.getHeight());
863
864 f32 dx = pos.X / (f32)screenSize.Width;
865 f32 dy = pos.Y / (f32)screenSize.Height;
866
867 if (camera->isOrthogonal())
868 ln.start = f->cameraPosition + (lefttoright * (dx-0.5f)) + (uptodown * (dy-0.5f));
869 else
870 ln.start = f->cameraPosition;
871
872 ln.end = farLeftUp + (lefttoright * dx) + (uptodown * dy);
873
874 return ln;
875}
876
877
878//! Calculates 2d screen position from a 3d position.
879core::position2d<s32> CSceneCollisionManager::getScreenCoordinatesFrom3DPosition(
880 const core::vector3df & pos3d, ICameraSceneNode* camera, bool useViewPort)
881{
882 if (!SceneManager || !Driver)
883 return core::position2d<s32>(-1000,-1000);
884
885 if (!camera)
886 camera = SceneManager->getActiveCamera();
887
888 if (!camera)
889 return core::position2d<s32>(-1000,-1000);
890
891 core::dimension2d<u32> dim;
892 if (useViewPort)
893 dim.set(Driver->getViewPort().getWidth(), Driver->getViewPort().getHeight());
894 else
895 dim=(Driver->getCurrentRenderTargetSize());
896
897 dim.Width /= 2;
898 dim.Height /= 2;
899
900 core::matrix4 trans = camera->getProjectionMatrix();
901 trans *= camera->getViewMatrix();
902
903 f32 transformedPos[4] = { pos3d.X, pos3d.Y, pos3d.Z, 1.0f };
904
905 trans.multiplyWith1x4Matrix(transformedPos);
906
907 if (transformedPos[3] < 0)
908 return core::position2d<s32>(-10000,-10000);
909
910 const f32 zDiv = transformedPos[3] == 0.0f ? 1.0f :
911 core::reciprocal(transformedPos[3]);
912
913 return core::position2d<s32>(
914 dim.Width + core::round32(dim.Width * (transformedPos[0] * zDiv)),
915 dim.Height - core::round32(dim.Height * (transformedPos[1] * zDiv)));
916}
917
918
919inline bool CSceneCollisionManager::getLowestRoot(f32 a, f32 b, f32 c, f32 maxR, f32* root)
920{
921 // check if solution exists
922 const f32 determinant = b*b - 4.0f*a*c;
923
924 // if determinant is negative, no solution
925 if (determinant < 0.0f || a == 0.f )
926 return false;
927
928 // calculate two roots: (if det==0 then x1==x2
929 // but lets disregard that slight optimization)
930
931 const f32 sqrtD = sqrtf(determinant);
932 const f32 invDA = core::reciprocal(2*a);
933 f32 r1 = (-b - sqrtD) * invDA;
934 f32 r2 = (-b + sqrtD) * invDA;
935
936 // sort so x1 <= x2
937 if (r1 > r2)
938 core::swap(r1,r2);
939
940 // get lowest root
941 if (r1 > 0 && r1 < maxR)
942 {
943 *root = r1;
944 return true;
945 }
946
947 // its possible that we want x2, this can happen if x1 < 0
948 if (r2 > 0 && r2 < maxR)
949 {
950 *root = r2;
951 return true;
952 }
953
954 return false;
955}
956
957
958} // end namespace scene
959} // end namespace irr
960
generated by cgit v1.1 at 2024-06-27 09:22:11 +0000