diff options
| author | cutealien <cutealien@dfc29bdd-3216-0410-991c-e03cc46cb475> | 2020-01-03 19:05:16 +0000 |
|---|---|---|
| committer | cutealien <cutealien@dfc29bdd-3216-0410-991c-e03cc46cb475> | 2020-01-03 19:05:16 +0000 |
| commit | 2ae2a551a6290f46734307bbfdafea4b1a2cf2ba (patch) | |
| tree | ba2f0b468640e44899fed3df2d4cc58795f4a003 /source/Irrlicht/CSceneCollisionManager.cpp | |
| download | irrlicht-2ae2a551a6290f46734307bbfdafea4b1a2cf2ba.tar.xz | |
Merging r5975 through r6036 from trunk to ogl-es branch.
GLES drivers adapted, but only did make compile-tests.
git-svn-id: svn://svn.code.sf.net/p/irrlicht/code/branches/ogl-es@6038 dfc29bdd-3216-0410-991c-e03cc46cb475
Diffstat (limited to 'source/Irrlicht/CSceneCollisionManager.cpp')
| -rw-r--r-- | source/Irrlicht/CSceneCollisionManager.cpp | 983 |
1 files changed, 983 insertions, 0 deletions
diff --git a/source/Irrlicht/CSceneCollisionManager.cpp b/source/Irrlicht/CSceneCollisionManager.cpp new file mode 100644 index 0000000..78ba131 --- /dev/null +++ b/source/Irrlicht/CSceneCollisionManager.cpp @@ -0,0 +1,983 @@ +// Copyright (C) 2002-2012 Nikolaus Gebhardt
+// This file is part of the "Irrlicht Engine".
+// For conditions of distribution and use, see copyright notice in irrlicht.h
+
+#include "CSceneCollisionManager.h"
+#include "ISceneNode.h"
+#include "ICameraSceneNode.h"
+#include "ITriangleSelector.h"
+#include "SViewFrustum.h"
+
+#include "os.h"
+#include "irrMath.h"
+
+namespace irr
+{
+namespace scene
+{
+
+//! constructor
+CSceneCollisionManager::CSceneCollisionManager(ISceneManager* smanager, video::IVideoDriver* driver)
+: SceneManager(smanager), Driver(driver)
+{
+ #ifdef _DEBUG
+ setDebugName("CSceneCollisionManager");
+ #endif
+
+ if (Driver)
+ Driver->grab();
+}
+
+
+//! destructor
+CSceneCollisionManager::~CSceneCollisionManager()
+{
+ if (Driver)
+ Driver->drop();
+}
+
+
+//! Returns the scene node, which is currently visible at the given
+//! screen coordinates, viewed from the currently active camera.
+ISceneNode* CSceneCollisionManager::getSceneNodeFromScreenCoordinatesBB(
+ const core::position2d<s32>& pos, s32 idBitMask, bool noDebugObjects, scene::ISceneNode* root)
+{
+ const core::line3d<f32> ln = getRayFromScreenCoordinates(pos, 0);
+
+ if ( ln.start == ln.end )
+ return 0;
+
+ return getSceneNodeFromRayBB(ln, idBitMask, noDebugObjects, root);
+}
+
+
+//! Returns the nearest scene node which collides with a 3d ray and
+//! which id matches a bitmask.
+ISceneNode* CSceneCollisionManager::getSceneNodeFromRayBB(
+ const core::line3d<f32>& ray,
+ s32 idBitMask, bool noDebugObjects, scene::ISceneNode* root)
+{
+ ISceneNode* best = 0;
+ f32 dist = FLT_MAX;
+
+ core::line3d<f32> truncatableRay(ray);
+
+ getPickedNodeBB((root==0)?SceneManager->getRootSceneNode():root, truncatableRay,
+ idBitMask, noDebugObjects, dist, best);
+
+ return best;
+}
+
+
+//! recursive method for going through all scene nodes
+void CSceneCollisionManager::getPickedNodeBB(ISceneNode* root,
+ core::line3df& ray, s32 bits, bool noDebugObjects,
+ f32& outbestdistance, ISceneNode*& outbestnode)
+{
+ const ISceneNodeList& children = root->getChildren();
+ const core::vector3df rayVector = ray.getVector().normalize();
+
+ ISceneNodeList::ConstIterator it = children.begin();
+ for (; it != children.end(); ++it)
+ {
+ ISceneNode* current = *it;
+
+ if (current->isVisible())
+ {
+ if((noDebugObjects ? !current->isDebugObject() : true) &&
+ (bits==0 || (bits != 0 && (current->getID() & bits))))
+ {
+ // Assume that single-point bounding-boxes are not meant for collision
+ const core::aabbox3df & objectBox = current->getBoundingBox();
+ if ( objectBox.isEmpty() )
+ continue;
+
+ // get world to object space transform
+ core::matrix4 worldToObject;
+ if (!current->getAbsoluteTransformation().getInverse(worldToObject))
+ continue;
+
+ // transform vector from world space to object space
+ core::line3df objectRay(ray);
+ worldToObject.transformVect(objectRay.start);
+ worldToObject.transformVect(objectRay.end);
+
+ // Do the initial intersection test in object space, since the
+ // object space box test is more accurate.
+ if(objectBox.isPointInside(objectRay.start))
+ {
+ // use fast bbox intersection to find distance to hitpoint
+ // algorithm from Kay et al., code from gamedev.net
+ const core::vector3df dir = (objectRay.end-objectRay.start).normalize();
+ const core::vector3df minDist = (objectBox.MinEdge - objectRay.start)/dir;
+ const core::vector3df maxDist = (objectBox.MaxEdge - objectRay.start)/dir;
+ const core::vector3df realMin(core::min_(minDist.X, maxDist.X),core::min_(minDist.Y, maxDist.Y),core::min_(minDist.Z, maxDist.Z));
+ const core::vector3df realMax(core::max_(minDist.X, maxDist.X),core::max_(minDist.Y, maxDist.Y),core::max_(minDist.Z, maxDist.Z));
+
+ const f32 minmax = core::min_(realMax.X, realMax.Y, realMax.Z);
+ // nearest distance to intersection
+ const f32 maxmin = core::max_(realMin.X, realMin.Y, realMin.Z);
+
+ const f32 toIntersectionSq = (maxmin>0?maxmin*maxmin:minmax*minmax);
+ if (toIntersectionSq < outbestdistance)
+ {
+ outbestdistance = toIntersectionSq;
+ outbestnode = current;
+
+ // And we can truncate the ray to stop us hitting further nodes.
+ ray.end = ray.start + (rayVector * sqrtf(toIntersectionSq));
+ }
+ }
+ else
+ if (objectBox.intersectsWithLine(objectRay))
+ {
+ // Now transform into world space, since we need to use world space
+ // scales and distances.
+ core::aabbox3df worldBox(objectBox);
+ current->getAbsoluteTransformation().transformBoxEx(worldBox);
+
+ core::vector3df edges[8];
+ worldBox.getEdges(edges);
+
+ /* We need to check against each of 6 faces, composed of these corners:
+ /3--------/7
+ / | / |
+ / | / |
+ 1---------5 |
+ | 2- - -| -6
+ | / | /
+ |/ | /
+ 0---------4/
+
+ Note that we define them as opposite pairs of faces.
+ */
+ static const s32 faceEdges[6][3] =
+ {
+ { 0, 1, 5 }, // Front
+ { 6, 7, 3 }, // Back
+ { 2, 3, 1 }, // Left
+ { 4, 5, 7 }, // Right
+ { 1, 3, 7 }, // Top
+ { 2, 0, 4 } // Bottom
+ };
+
+ core::vector3df intersection;
+ core::plane3df facePlane;
+ f32 bestDistToBoxBorder = FLT_MAX;
+ f32 bestToIntersectionSq = FLT_MAX;
+
+ for(s32 face = 0; face < 6; ++face)
+ {
+ facePlane.setPlane(edges[faceEdges[face][0]],
+ edges[faceEdges[face][1]],
+ edges[faceEdges[face][2]]);
+
+ // Only consider lines that might be entering through this face, since we
+ // already know that the start point is outside the box.
+ if(facePlane.classifyPointRelation(ray.start) != core::ISREL3D_FRONT)
+ continue;
+
+ // Don't bother using a limited ray, since we already know that it should be long
+ // enough to intersect with the box.
+ if(facePlane.getIntersectionWithLine(ray.start, rayVector, intersection))
+ {
+ const f32 toIntersectionSq = ray.start.getDistanceFromSQ(intersection);
+ if(toIntersectionSq < outbestdistance)
+ {
+ // We have to check that the intersection with this plane is actually
+ // on the box, so need to go back to object space again.
+ worldToObject.transformVect(intersection);
+
+ // find the closest point on the box borders. Have to do this as exact checks will fail due to floating point problems.
+ f32 distToBorder = core::max_ ( core::min_ (core::abs_(objectBox.MinEdge.X-intersection.X), core::abs_(objectBox.MaxEdge.X-intersection.X)),
+ core::min_ (core::abs_(objectBox.MinEdge.Y-intersection.Y), core::abs_(objectBox.MaxEdge.Y-intersection.Y)),
+ core::min_ (core::abs_(objectBox.MinEdge.Z-intersection.Z), core::abs_(objectBox.MaxEdge.Z-intersection.Z)) );
+ if ( distToBorder < bestDistToBoxBorder )
+ {
+ bestDistToBoxBorder = distToBorder;
+ bestToIntersectionSq = toIntersectionSq;
+ }
+ }
+ }
+
+ // If the ray could be entering through the first face of a pair, then it can't
+ // also be entering through the opposite face, and so we can skip that face.
+ if (!(face & 0x01))
+ ++face;
+ }
+
+ if ( bestDistToBoxBorder < FLT_MAX )
+ {
+ outbestdistance = bestToIntersectionSq;
+ outbestnode = current;
+
+ // If we got a hit, we can now truncate the ray to stop us hitting further nodes.
+ ray.end = ray.start + (rayVector * sqrtf(outbestdistance));
+ }
+ }
+ }
+
+ // Only check the children if this node is visible.
+ getPickedNodeBB(current, ray, bits, noDebugObjects, outbestdistance, outbestnode);
+ }
+ }
+}
+
+
+ISceneNode* CSceneCollisionManager::getSceneNodeAndCollisionPointFromRay(
+ SCollisionHit& hitResult,
+ const core::line3df& ray,
+ s32 idBitMask,
+ ISceneNode * collisionRootNode,
+ bool noDebugObjects)
+{
+ if(0 == collisionRootNode)
+ collisionRootNode = SceneManager->getRootSceneNode();
+
+ // We don't try to do anything too clever, like sorting the candidate
+ // nodes by distance to bounding-box. In the example below, we could do the
+ // triangle collision check with node A first, but we'd have to check node B
+ // anyway, as the actual collision point could be (and is) closer than the
+ // collision point in node A.
+ //
+ // ray end
+ // |
+ // AAAAAAAAAA
+ // A |
+ // A | B
+ // A | B
+ // A BBBBB
+ // A |
+ // A |
+ // |
+ // |
+ // ray start
+ //
+ // We therefore have to do a full BB and triangle collision on every scene
+ // node in order to find the nearest collision point, so sorting them by
+ // bounding box would be pointless.
+
+ f32 bestDistanceSquared = FLT_MAX;
+ core::line3df rayRest(ray);
+ getPickedNodeFromBBAndSelector(hitResult, collisionRootNode, rayRest, idBitMask,
+ noDebugObjects, bestDistanceSquared);
+ return hitResult.Node;
+}
+
+
+void CSceneCollisionManager::getPickedNodeFromBBAndSelector(
+ SCollisionHit& hitResult,
+ ISceneNode * root,
+ core::line3df & ray,
+ s32 bits,
+ bool noDebugObjects,
+ f32 & outBestDistanceSquared)
+{
+ const ISceneNodeList& children = root->getChildren();
+
+ ISceneNodeList::ConstIterator it = children.begin();
+ for (; it != children.end(); ++it)
+ {
+ ISceneNode* current = *it;
+ ITriangleSelector * selector = current->getTriangleSelector();
+
+ if (selector && current->isVisible() &&
+ (noDebugObjects ? !current->isDebugObject() : true) &&
+ (bits==0 || (bits != 0 && (current->getID() & bits))))
+ {
+ // get world to object space transform
+ core::matrix4 mat;
+ if (!current->getAbsoluteTransformation().getInverse(mat))
+ continue;
+
+ // transform vector from world space to object space
+ core::line3df line(ray);
+ mat.transformVect(line.start);
+ mat.transformVect(line.end);
+
+ const core::aabbox3df& box = current->getBoundingBox();
+
+ SCollisionHit candidateHitResult;
+
+ // do intersection test in object space
+ if (box.intersectsWithLine(line) &&
+ getCollisionPoint(candidateHitResult, ray, selector))
+ {
+ const f32 distanceSquared = (candidateHitResult.Intersection - ray.start).getLengthSQ();
+
+ if(distanceSquared < outBestDistanceSquared)
+ {
+ outBestDistanceSquared = distanceSquared;
+ hitResult = candidateHitResult;
+ const core::vector3df rayVector = ray.getVector().normalize();
+ ray.end = ray.start + (rayVector * sqrtf(distanceSquared));
+ }
+ }
+ }
+
+ getPickedNodeFromBBAndSelector(hitResult, current, ray, bits, noDebugObjects,
+ outBestDistanceSquared);
+ }
+}
+
+
+//! Returns the scene node, at which the given camera is looking at and
+//! which id matches the bitmask.
+ISceneNode* CSceneCollisionManager::getSceneNodeFromCameraBB(
+ const ICameraSceneNode* camera, s32 idBitMask, bool noDebugObjects)
+{
+ if (!camera)
+ return 0;
+
+ const core::vector3df start = camera->getAbsolutePosition();
+ core::vector3df end = camera->getTarget();
+
+ end = start + ((end - start).normalize() * camera->getFarValue());
+
+ return getSceneNodeFromRayBB(core::line3d<f32>(start, end), idBitMask, noDebugObjects);
+}
+
+bool CSceneCollisionManager::getCollisionPoint(SCollisionHit& hitResult, const core::line3d<f32>& ray, ITriangleSelector* selector)
+{
+ if (!selector)
+ {
+ return false;
+ }
+
+ s32 totalcnt = selector->getTriangleCount();
+ if ( totalcnt <= 0 )
+ return false;
+
+ Triangles.set_used(totalcnt);
+
+ s32 cnt = 0;
+ irr::core::array<SCollisionTriangleRange> outTriangleInfo;
+ selector->getTriangles(Triangles.pointer(), totalcnt, cnt, ray, 0, true, &outTriangleInfo);
+
+ const core::vector3df linevect = ray.getVector().normalize();
+ core::vector3df intersection;
+ f32 nearest = FLT_MAX;
+ irr::s32 foundIndex = -1;
+ const f32 raylength = ray.getLengthSQ();
+
+ const f32 minX = core::min_(ray.start.X, ray.end.X);
+ const f32 maxX = core::max_(ray.start.X, ray.end.X);
+ const f32 minY = core::min_(ray.start.Y, ray.end.Y);
+ const f32 maxY = core::max_(ray.start.Y, ray.end.Y);
+ const f32 minZ = core::min_(ray.start.Z, ray.end.Z);
+ const f32 maxZ = core::max_(ray.start.Z, ray.end.Z);
+
+ for (s32 i=0; i<cnt; ++i)
+ {
+ const core::triangle3df & triangle = Triangles[i];
+
+ if(minX > triangle.pointA.X && minX > triangle.pointB.X && minX > triangle.pointC.X)
+ continue;
+ if(maxX < triangle.pointA.X && maxX < triangle.pointB.X && maxX < triangle.pointC.X)
+ continue;
+ if(minY > triangle.pointA.Y && minY > triangle.pointB.Y && minY > triangle.pointC.Y)
+ continue;
+ if(maxY < triangle.pointA.Y && maxY < triangle.pointB.Y && maxY < triangle.pointC.Y)
+ continue;
+ if(minZ > triangle.pointA.Z && minZ > triangle.pointB.Z && minZ > triangle.pointC.Z)
+ continue;
+ if(maxZ < triangle.pointA.Z && maxZ < triangle.pointB.Z && maxZ < triangle.pointC.Z)
+ continue;
+
+ if (triangle.getIntersectionWithLine(ray.start, linevect, intersection))
+ {
+ const f32 tmp = intersection.getDistanceFromSQ(ray.start);
+ const f32 tmp2 = intersection.getDistanceFromSQ(ray.end);
+
+ if (tmp < raylength && tmp2 < raylength && tmp < nearest)
+ {
+ nearest = tmp;
+
+ hitResult.Triangle = triangle;
+ hitResult.Intersection = intersection;
+ foundIndex = i;
+ }
+ }
+ }
+
+ if ( foundIndex >= 0 )
+ {
+ for ( irr::u32 t=0; t<outTriangleInfo.size(); ++t )
+ {
+ if ( outTriangleInfo[t].isIndexInRange(foundIndex) )
+ {
+ hitResult.Node = outTriangleInfo[t].SceneNode;
+ hitResult.MeshBuffer = outTriangleInfo[t].MeshBuffer;
+ hitResult.MaterialIndex = outTriangleInfo[t].MaterialIndex;
+ hitResult.TriangleSelector = outTriangleInfo[t].Selector;
+
+ break;
+ }
+ }
+
+ return true;
+ }
+
+ return false;
+}
+
+//! Collides a moving ellipsoid with a 3d world with gravity and returns
+//! the resulting new position of the ellipsoid.
+core::vector3df CSceneCollisionManager::getCollisionResultPosition(
+ ITriangleSelector* selector,
+ const core::vector3df &position, const core::vector3df& radius,
+ const core::vector3df& direction,
+ core::triangle3df& triout,
+ core::vector3df& hitPosition,
+ bool& outFalling,
+ ISceneNode*& outNode,
+ f32 slidingSpeed,
+ const core::vector3df& gravity)
+{
+ return collideEllipsoidWithWorld(selector, position,
+ radius, direction, slidingSpeed, gravity, triout, hitPosition, outFalling, outNode);
+}
+
+
+bool CSceneCollisionManager::testTriangleIntersection(SCollisionData* colData,
+ const core::triangle3df& triangle)
+{
+ const core::plane3d<f32> trianglePlane = triangle.getPlane();
+
+ // only check front facing polygons
+ if ( !trianglePlane.isFrontFacing(colData->normalizedVelocity) )
+ return false;
+
+ // get interval of plane intersection
+
+ f32 t1, t0;
+ bool embeddedInPlane = false;
+
+ // calculate signed distance from sphere position to triangle plane
+ f32 signedDistToTrianglePlane = trianglePlane.getDistanceTo(
+ colData->basePoint);
+
+ f32 normalDotVelocity =
+ trianglePlane.Normal.dotProduct(colData->velocity);
+
+ if ( core::iszero ( normalDotVelocity ) )
+ {
+ // sphere is traveling parallel to plane
+
+ if (fabs(signedDistToTrianglePlane) >= 1.0f)
+ return false; // no collision possible
+ else
+ {
+ // sphere is embedded in plane
+ embeddedInPlane = true;
+ t0 = 0.0;
+ t1 = 1.0;
+ }
+ }
+ else
+ {
+ normalDotVelocity = core::reciprocal ( normalDotVelocity );
+
+ // N.D is not 0. Calculate intersection interval
+ t0 = (-1.f - signedDistToTrianglePlane) * normalDotVelocity;
+ t1 = (1.f - signedDistToTrianglePlane) * normalDotVelocity;
+
+ // Swap so t0 < t1
+ if (t0 > t1) { f32 tmp = t1; t1 = t0; t0 = tmp; }
+
+ // check if at least one value is within the range
+ if (t0 > 1.0f || t1 < 0.0f)
+ return false; // both t values are outside 1 and 0, no collision possible
+
+ // clamp to 0 and 1
+ t0 = core::clamp ( t0, 0.f, 1.f );
+ t1 = core::clamp ( t1, 0.f, 1.f );
+ }
+
+ // at this point we have t0 and t1, if there is any intersection, it
+ // is between this interval
+ core::vector3df collisionPoint;
+ bool foundCollision = false;
+ f32 t = 1.0f;
+
+ // first check the easy case: Collision within the triangle;
+ // if this happens, it must be at t0 and this is when the sphere
+ // rests on the front side of the triangle plane. This can only happen
+ // if the sphere is not embedded in the triangle plane.
+
+ if (!embeddedInPlane)
+ {
+ core::vector3df planeIntersectionPoint =
+ (colData->basePoint - trianglePlane.Normal)
+ + (colData->velocity * t0);
+
+ if (triangle.isPointInside(planeIntersectionPoint))
+ {
+ foundCollision = true;
+ t = t0;
+ collisionPoint = planeIntersectionPoint;
+ }
+ }
+
+ // if we haven't found a collision already we will have to sweep
+ // the sphere against points and edges of the triangle. Note: A
+ // collision inside the triangle will always happen before a
+ // vertex or edge collision.
+
+ if (!foundCollision)
+ {
+ core::vector3df velocity = colData->velocity;
+ core::vector3df base = colData->basePoint;
+
+ f32 velocitySqaredLength = velocity.getLengthSQ();
+ f32 a,b,c;
+ f32 newT;
+
+ // for each edge or vertex a quadratic equation has to be solved:
+ // a*t^2 + b*t + c = 0. We calculate a,b, and c for each test.
+
+ // check against points
+ a = velocitySqaredLength;
+
+ // p1
+ b = 2.0f * (velocity.dotProduct(base - triangle.pointA));
+ c = (triangle.pointA-base).getLengthSQ() - 1.f;
+ if (getLowestRoot(a,b,c,t, &newT))
+ {
+ t = newT;
+ foundCollision = true;
+ collisionPoint = triangle.pointA;
+ }
+
+ // p2
+ if (!foundCollision)
+ {
+ b = 2.0f * (velocity.dotProduct(base - triangle.pointB));
+ c = (triangle.pointB-base).getLengthSQ() - 1.f;
+ if (getLowestRoot(a,b,c,t, &newT))
+ {
+ t = newT;
+ foundCollision = true;
+ collisionPoint = triangle.pointB;
+ }
+ }
+
+ // p3
+ if (!foundCollision)
+ {
+ b = 2.0f * (velocity.dotProduct(base - triangle.pointC));
+ c = (triangle.pointC-base).getLengthSQ() - 1.f;
+ if (getLowestRoot(a,b,c,t, &newT))
+ {
+ t = newT;
+ foundCollision = true;
+ collisionPoint = triangle.pointC;
+ }
+ }
+
+ // check against edges:
+
+ // p1 --- p2
+ core::vector3df edge = triangle.pointB - triangle.pointA;
+ core::vector3df baseToVertex = triangle.pointA - base;
+ f32 edgeSqaredLength = edge.getLengthSQ();
+ f32 edgeDotVelocity = edge.dotProduct(velocity);
+ f32 edgeDotBaseToVertex = edge.dotProduct(baseToVertex);
+
+ // calculate parameters for equation
+ a = edgeSqaredLength* -velocitySqaredLength +
+ edgeDotVelocity*edgeDotVelocity;
+ b = edgeSqaredLength* (2.f *velocity.dotProduct(baseToVertex)) -
+ 2.0f*edgeDotVelocity*edgeDotBaseToVertex;
+ c = edgeSqaredLength* (1.f -baseToVertex.getLengthSQ()) +
+ edgeDotBaseToVertex*edgeDotBaseToVertex;
+
+ // does the swept sphere collide against infinite edge?
+ if (getLowestRoot(a,b,c,t,&newT))
+ {
+ f32 f = (edgeDotVelocity*newT - edgeDotBaseToVertex) / edgeSqaredLength;
+ if (f >=0.0f && f <= 1.0f)
+ {
+ // intersection took place within segment
+ t = newT;
+ foundCollision = true;
+ collisionPoint = triangle.pointA + (edge*f);
+ }
+ }
+
+ // p2 --- p3
+ edge = triangle.pointC-triangle.pointB;
+ baseToVertex = triangle.pointB - base;
+ edgeSqaredLength = edge.getLengthSQ();
+ edgeDotVelocity = edge.dotProduct(velocity);
+ edgeDotBaseToVertex = edge.dotProduct(baseToVertex);
+
+ // calculate parameters for equation
+ a = edgeSqaredLength* -velocitySqaredLength +
+ edgeDotVelocity*edgeDotVelocity;
+ b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) -
+ 2.0f*edgeDotVelocity*edgeDotBaseToVertex;
+ c = edgeSqaredLength* (1-baseToVertex.getLengthSQ()) +
+ edgeDotBaseToVertex*edgeDotBaseToVertex;
+
+ // does the swept sphere collide against infinite edge?
+ if (getLowestRoot(a,b,c,t,&newT))
+ {
+ f32 f = (edgeDotVelocity*newT-edgeDotBaseToVertex) /
+ edgeSqaredLength;
+ if (f >=0.0f && f <= 1.0f)
+ {
+ // intersection took place within segment
+ t = newT;
+ foundCollision = true;
+ collisionPoint = triangle.pointB + (edge*f);
+ }
+ }
+
+
+ // p3 --- p1
+ edge = triangle.pointA-triangle.pointC;
+ baseToVertex = triangle.pointC - base;
+ edgeSqaredLength = edge.getLengthSQ();
+ edgeDotVelocity = edge.dotProduct(velocity);
+ edgeDotBaseToVertex = edge.dotProduct(baseToVertex);
+
+ // calculate parameters for equation
+ a = edgeSqaredLength* -velocitySqaredLength +
+ edgeDotVelocity*edgeDotVelocity;
+ b = edgeSqaredLength* (2*velocity.dotProduct(baseToVertex)) -
+ 2.0f*edgeDotVelocity*edgeDotBaseToVertex;
+ c = edgeSqaredLength* (1-baseToVertex.getLengthSQ()) +
+ edgeDotBaseToVertex*edgeDotBaseToVertex;
+
+ // does the swept sphere collide against infinite edge?
+ if (getLowestRoot(a,b,c,t,&newT))
+ {
+ f32 f = (edgeDotVelocity*newT-edgeDotBaseToVertex) /
+ edgeSqaredLength;
+ if (f >=0.0f && f <= 1.0f)
+ {
+ // intersection took place within segment
+ t = newT;
+ foundCollision = true;
+ collisionPoint = triangle.pointC + (edge*f);
+ }
+ }
+ }// end no collision found
+
+ // set result:
+ if (foundCollision)
+ {
+ ++colData->triangleHits;
+
+ // distance to collision is t
+ f32 distToCollision = t*colData->velocity.getLength();
+
+ // does this triangle qualify for closest hit?
+ if (!colData->foundCollision ||
+ distToCollision < colData->nearestDistance)
+ {
+ colData->nearestDistance = distToCollision;
+ colData->intersectionPoint = collisionPoint;
+ colData->foundCollision = true;
+ colData->intersectionTriangle = triangle;
+ return true;
+ }
+ }// end found collision
+
+ return false;
+}
+
+
+//! Collides a moving ellipsoid with a 3d world with gravity and returns
+//! the resulting new position of the ellipsoid.
+core::vector3df CSceneCollisionManager::collideEllipsoidWithWorld(
+ ITriangleSelector* selector, const core::vector3df &position,
+ const core::vector3df& radius, const core::vector3df& velocity,
+ f32 slidingSpeed,
+ const core::vector3df& gravity,
+ core::triangle3df& triout,
+ core::vector3df& hitPosition,
+ bool& outFalling,
+ ISceneNode*& outNode)
+{
+ if (!selector || radius.X == 0.0f || radius.Y == 0.0f || radius.Z == 0.0f)
+ return position;
+
+ // This code is based on the paper "Improved Collision detection and Response"
+ // by Kasper Fauerby, but some parts are modified.
+
+ SCollisionData colData;
+ colData.R3Position = position;
+ colData.R3Velocity = velocity;
+ colData.eRadius = radius;
+ colData.nearestDistance = FLT_MAX;
+ colData.selector = selector;
+ colData.slidingSpeed = slidingSpeed;
+ colData.triangleHits = 0;
+ colData.node = 0;
+
+ core::vector3df eSpacePosition = colData.R3Position / colData.eRadius;
+ core::vector3df eSpaceVelocity = colData.R3Velocity / colData.eRadius;
+
+ // iterate until we have our final position
+
+ core::vector3df finalPos = collideWithWorld(
+ 0, colData, eSpacePosition, eSpaceVelocity);
+
+ outFalling = false;
+
+ // add gravity
+
+ if (gravity != core::vector3df(0,0,0))
+ {
+ colData.R3Position = finalPos * colData.eRadius;
+ colData.R3Velocity = gravity;
+ colData.triangleHits = 0;
+
+ eSpaceVelocity = gravity/colData.eRadius;
+
+ finalPos = collideWithWorld(0, colData,
+ finalPos, eSpaceVelocity);
+
+ outFalling = (colData.triangleHits == 0);
+ }
+
+ if (colData.triangleHits)
+ {
+ triout = colData.intersectionTriangle;
+ triout.pointA *= colData.eRadius;
+ triout.pointB *= colData.eRadius;
+ triout.pointC *= colData.eRadius;
+ outNode = colData.node;
+ }
+
+ finalPos *= colData.eRadius;
+ hitPosition = colData.intersectionPoint * colData.eRadius;
+ return finalPos;
+}
+
+
+core::vector3df CSceneCollisionManager::collideWithWorld(s32 recursionDepth,
+ SCollisionData &colData, const core::vector3df& pos, const core::vector3df& vel)
+{
+ f32 veryCloseDistance = colData.slidingSpeed;
+
+ if (recursionDepth > 5)
+ return pos;
+
+ colData.velocity = vel;
+ colData.normalizedVelocity = vel;
+ colData.normalizedVelocity.normalize();
+ colData.basePoint = pos;
+ colData.foundCollision = false;
+ colData.nearestDistance = FLT_MAX;
+
+ //------------------ collide with world
+
+ // get all triangles with which we might collide
+ core::aabbox3d<f32> box(colData.R3Position);
+ box.addInternalPoint(colData.R3Position + colData.R3Velocity);
+ box.MinEdge -= colData.eRadius;
+ box.MaxEdge += colData.eRadius;
+
+ s32 totalTriangleCnt = colData.selector->getTriangleCount();
+ Triangles.set_used(totalTriangleCnt);
+
+ core::matrix4 scaleMatrix;
+ scaleMatrix.setScale(
+ core::vector3df(1.0f / colData.eRadius.X,
+ 1.0f / colData.eRadius.Y,
+ 1.0f / colData.eRadius.Z));
+
+ irr::core::array<SCollisionTriangleRange> outTriangleInfo;
+ s32 triangleCnt = 0;
+ colData.selector->getTriangles(Triangles.pointer(), totalTriangleCnt, triangleCnt, box, &scaleMatrix, true, &outTriangleInfo);
+
+ // Find closest intersection
+ irr::s32 nearestTriangleIndex = -1;
+ for (s32 i=0; i<triangleCnt; ++i)
+ {
+ if(testTriangleIntersection(&colData, Triangles[i]))
+ {
+ nearestTriangleIndex = i;
+ }
+ }
+ if ( nearestTriangleIndex >= 0 )
+ {
+ for ( irr::u32 t=0; t<outTriangleInfo.size(); ++t )
+ {
+ if ( outTriangleInfo[t].isIndexInRange(nearestTriangleIndex) )
+ {
+ colData.node = outTriangleInfo[t].SceneNode;
+ break;
+ }
+ }
+ }
+
+ //---------------- end collide with world
+
+ if (!colData.foundCollision)
+ return pos + vel;
+
+ // original destination point
+ const core::vector3df destinationPoint = pos + vel;
+ core::vector3df newBasePoint = pos;
+
+ // only update if we are not already very close
+ // and if so only move very close to intersection, not to the
+ // exact point
+ if (colData.nearestDistance >= veryCloseDistance)
+ {
+ core::vector3df v = vel;
+ v.setLength( colData.nearestDistance - veryCloseDistance );
+ newBasePoint = colData.basePoint + v;
+
+ v.normalize();
+ colData.intersectionPoint -= (v * veryCloseDistance);
+ }
+
+ // calculate sliding plane
+
+ const core::vector3df slidePlaneOrigin = colData.intersectionPoint;
+ const core::vector3df slidePlaneNormal = (newBasePoint - colData.intersectionPoint).normalize();
+ core::plane3d<f32> slidingPlane(slidePlaneOrigin, slidePlaneNormal);
+
+ core::vector3df newDestinationPoint =
+ destinationPoint -
+ (slidePlaneNormal * slidingPlane.getDistanceTo(destinationPoint));
+
+ // generate slide vector
+
+ const core::vector3df newVelocityVector = newDestinationPoint -
+ colData.intersectionPoint;
+
+ if (newVelocityVector.getLength() < veryCloseDistance)
+ return newBasePoint;
+
+ return collideWithWorld(recursionDepth+1, colData,
+ newBasePoint, newVelocityVector);
+}
+
+
+//! Returns a 3d ray which would go through the 2d screen coordinates.
+core::line3d<f32> CSceneCollisionManager::getRayFromScreenCoordinates(
+ const core::position2d<s32> & pos, const ICameraSceneNode* camera)
+{
+ core::line3d<f32> ln(0,0,0,0,0,0);
+
+ if (!SceneManager)
+ return ln;
+
+ if (!camera)
+ camera = SceneManager->getActiveCamera();
+
+ if (!camera)
+ return ln;
+
+ const scene::SViewFrustum* f = camera->getViewFrustum();
+
+ core::vector3df farLeftUp = f->getFarLeftUp();
+ core::vector3df lefttoright = f->getFarRightUp() - farLeftUp;
+ core::vector3df uptodown = f->getFarLeftDown() - farLeftUp;
+
+ const core::rect<s32>& viewPort = Driver->getViewPort();
+ core::dimension2d<u32> screenSize(viewPort.getWidth(), viewPort.getHeight());
+
+ f32 dx = pos.X / (f32)screenSize.Width;
+ f32 dy = pos.Y / (f32)screenSize.Height;
+
+ if (camera->isOrthogonal())
+ ln.start = f->cameraPosition + (lefttoright * (dx-0.5f)) + (uptodown * (dy-0.5f));
+ else
+ ln.start = f->cameraPosition;
+
+ ln.end = farLeftUp + (lefttoright * dx) + (uptodown * dy);
+
+ return ln;
+}
+
+
+//! Calculates 2d screen position from a 3d position.
+core::position2d<s32> CSceneCollisionManager::getScreenCoordinatesFrom3DPosition(
+ const core::vector3df & pos3d, const ICameraSceneNode* camera, bool useViewPort)
+{
+ if (!SceneManager || !Driver)
+ return core::position2d<s32>(-1000,-1000);
+
+ if (!camera)
+ camera = SceneManager->getActiveCamera();
+
+ if (!camera)
+ return core::position2d<s32>(-1000,-1000);
+
+ core::dimension2d<u32> dim;
+ if (useViewPort)
+ dim.set(Driver->getViewPort().getWidth(), Driver->getViewPort().getHeight());
+ else
+ dim=(Driver->getCurrentRenderTargetSize());
+
+ dim.Width /= 2;
+ dim.Height /= 2;
+
+ core::matrix4 trans = camera->getProjectionMatrix();
+ trans *= camera->getViewMatrix();
+
+ f32 transformedPos[4] = { pos3d.X, pos3d.Y, pos3d.Z, 1.0f };
+
+ trans.multiplyWith1x4Matrix(transformedPos);
+
+ if (transformedPos[3] < 0)
+ return core::position2d<s32>(-10000,-10000);
+
+ const f32 zDiv = transformedPos[3] == 0.0f ? 1.0f :
+ core::reciprocal(transformedPos[3]);
+
+ return core::position2d<s32>(
+ dim.Width + core::round32(dim.Width * (transformedPos[0] * zDiv)),
+ dim.Height - core::round32(dim.Height * (transformedPos[1] * zDiv)));
+}
+
+
+inline bool CSceneCollisionManager::getLowestRoot(f32 a, f32 b, f32 c, f32 maxR, f32* root) const
+{
+ // check if solution exists
+ const f32 determinant = b*b - 4.0f*a*c;
+
+ // if determinant is negative, no solution
+ if (determinant < 0.0f || a == 0.f )
+ return false;
+
+ // calculate two roots: (if det==0 then x1==x2
+ // but lets disregard that slight optimization)
+
+ const f32 sqrtD = sqrtf(determinant);
+ const f32 invDA = core::reciprocal(2*a);
+ f32 r1 = (-b - sqrtD) * invDA;
+ f32 r2 = (-b + sqrtD) * invDA;
+
+ // sort so x1 <= x2
+ if (r1 > r2)
+ core::swap(r1,r2);
+
+ // get lowest root
+ if (r1 > 0 && r1 < maxR)
+ {
+ *root = r1;
+ return true;
+ }
+
+ // its possible that we want x2, this can happen if x1 < 0
+ if (r2 > 0 && r2 < maxR)
+ {
+ *root = r2;
+ return true;
+ }
+
+ return false;
+}
+
+
+} // end namespace scene
+} // end namespace irr
+
|