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);
}
