core::vector3df collideWithWorld(s32 recursionDepth, SCollisionData &colData, core::vector3df pos, 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; double uhel_cos = 90 - acos(colData.normalizedVelocity.dotProduct(vector3df(0, -1, 0).normalize())) * 180.0 / PI; if (recursionDepth > 0 && vel.getLength() > 0 && vel.Y < 0) { if (abs(uhel_cos) < 50) return pos; } //------------------ 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)); s32 triangleCnt = 0; colData.selector->getTriangles(Triangles.pointer(), totalTriangleCnt, triangleCnt, box, &scaleMatrix); for (s32 i=0; i<triangleCnt; ++i) if(testTriangleIntersection(&colData, Triangles[i])) colData.triangleIndex = i; //---------------- end collide with world if (!colData.foundCollision) return pos + vel; // original destination point const core::vector3df destinationPoint = pos + vel; core::vector3df newBasePoint = pos; 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; //printf("Puvodni delka: %f | nova delka: %f\n", colData.velocity.getLength(), newVelocityVector.getLength()); return collideWithWorld(recursionDepth+1, colData, newBasePoint, newVelocityVector); }