bool IntersectedPlane(CVector3 vPoly[], CVector3 vLine[], CVector3 &vNormal, float &originDistance)
{
float distance1=0, distance2=0; // The distances from the 2 points of the line from the plane
vNormal = Normal(vPoly); // We need to get the normal of our plane to go any further
// Let's find the distance our plane is from the origin. We can find this value
// from the normal to the plane (polygon) and any point that lies on that plane (Any vertex)
originDistance = PlaneDistance(vNormal, vPoly[0]);
// Get the distance from point1 from the plane using: Ax + By + Cz + D = (The distance from the plane)
distance1 = ((vNormal.x * vLine[0].x) + // Ax +
(vNormal.y * vLine[0].y) + // Bx +
(vNormal.z * vLine[0].z)) + originDistance; // Cz + D
// Get the distance from point2 from the plane using Ax + By + Cz + D = (The distance from the plane)
distance2 = ((vNormal.x * vLine[1].x) + // Ax +
(vNormal.y * vLine[1].y) + // Bx +
(vNormal.z * vLine[1].z)) + originDistance; // Cz + D
// Now that we have 2 distances from the plane, if we times them together we either
// get a positive or negative number. If it's a negative number, that means we collided!
// This is because the 2 points must be on either side of the plane (IE. -1 * 1 = -1).
if(distance1 * distance2 >= 0) // Check to see if both point's distances are both negative or both positive
return false; // Return false if each point has the same sign. -1 and 1 would mean each point is on either side of the plane. -1 -2 or 3 4 wouldn't...
return true; // The line intersected the plane, Return TRUE
}
vec AseFile::CheckSimpleCollision(const vec &vp, float radius)
{
vec newvp = vp;
if(!SphereInBox( vp, radius, min, max))
return newvp;
for(int i=0; i<objects.size(); i++)
{
zASE_Object *obj = &objects[i];
if(!SphereInBox( newvp, radius, obj->min, obj->max))continue;
for( int j=0; j<obj->numOfFaces; j++)
{
zASE_Face *ind = &obj->pFaces[j];
if( SpherePolygonCollisionRadius05( newvp, radius, obj->pVerts[ind->index[0]], obj->pVerts[ind->index[1]], obj->pVerts[ind->index[2]], obj->pFaceNormals[j] ) )
{
float distanceCenterToPlane = DOT3( obj->pFaceNormals[j], newvp) + PlaneDistance( obj->pFaceNormals[j], obj->pVerts[ind->index[0]] ); // d = Ax + By + Cz + D
if(distanceCenterToPlane>0)
newvp += (radius-distanceCenterToPlane)*obj->pFaceNormals[j];
else
newvp -= (radius+distanceCenterToPlane)*obj->pFaceNormals[j];
}
}
}
return newvp;
}
bool IntersectedPlane(Vector3 vPoly[], Vector3 vLine[], Vector3 &vNormal, float &originDistance)//线段与平面是否相交
{
float distance1=0, distance2=0;
vNormal = Normal(vPoly);
originDistance = PlaneDistance(vNormal, vPoly[0]);
distance1 = vNormal.dotProduct(vLine[0]);
distance2 = vNormal.dotProduct(vLine[1]);
if(distance1 * distance2 >= 0) //如果两个端点在平面同侧，不相叫，否则相交
return false;
return true;
}
int ClassifySphere(Vector3 &vCenter,
Vector3 &vNormal, Vector3 &vPoint, float radius, float &distance)
{ //球体与平面的关系
float d = (float)PlaneDistance(vNormal, vPoint);
distance = (vNormal.dotProduct(vCenter)+ d);
if(Absolute(distance) < radius) //绝对值小于半径，相交
return INTERSECTS;
else if(distance >= radius) //大于或等于半径在前面
return FRONT;
return BEHIND;
}
Vector3 IntersectionPoint(Vector3 vNormal, Vector3 vLine[], double distance)//计算交点
{
distance=PlaneDistance(vNormal, vLine[0]);
Vector3 vPoint, vLineDir;
double Numerator = 0.0, Denominator = 0.0, dist = 0.0;
vLineDir = vLine[1] - vLine[0];
vLineDir = Normalize(vLineDir);
Numerator = - (vNormal.dotProduct(vLine[0])+ distance);
Denominator =vNormal.dotProduct(vLineDir);
if( Denominator == 0.0)
return vLine[0];
dist = Numerator / Denominator;
vPoint.x = (float)(vLine[0].x + (vLineDir.x * dist));
vPoint.y = (float)(vLine[0].y + (vLineDir.y * dist));
vPoint.z = (float)(vLine[0].z + (vLineDir.z * dist));
return vPoint;
}
int ClassifySphere(CVector3 &vCenter,
CVector3 &vNormal, CVector3 &vPoint, float radius, float &distance)
{
// First we need to find the distance our polygon plane is from the origin.
float d = (float)PlaneDistance(vNormal, vPoint);
// Here we use the famous distance formula to find the distance the center point
// of the sphere is from the polygon's plane.
distance = (vNormal.x * vCenter.x + vNormal.y * vCenter.y + vNormal.z * vCenter.z + d);
// If the absolute value of the distance we just found is less than the radius,
// the sphere intersected the plane.
if(Absolute(distance) < radius)
return INTERSECTS;
// Else, if the distance is greater than or equal to the radius, the sphere is
// completely in FRONT of the plane.
else if(distance >= radius)
return FRONT;
// If the sphere isn't intersecting or in FRONT of the plane, it must be BEHIND
return BEHIND;
}
bool IntersectedPlane(CVector3 vTriangle[], CVector3 vLine[])
{
float distance1=0, distance2=0; // The distances from the 2 points of the line from the plane
CVector3 vNormal = Normal(vTriangle); // We need to get the normal of our plane to go any further
// Now that we have the normal, we need to calculate the distance our triangle is from the origin.
// Since we would have the same triangle, but -10 down the z axis, we need to know
// how far our plane is to the origin. The origin is (0, 0, 0), so we need to find
// the shortest distance our plane is from (0, 0, 0). This way we can test the collision.
// The direction the plane is facing is important (We know this by the normal), but it's
// also important WHERE that plane is in our 3D space. I hope this makes sense.
// We created a function to calculate the distance for us. All we need is the normal
// of the plane, and then ANY point located on that plane. Well, we have 3 points. Each
// point of the triangle is on the plane, so we just pass in one of our points. It doesn't
// matter which one, so we will just pass in the first one. We get a single value back.
// That is the distance. Just like our normalized normal is of length 1, our distance
// is a single value too. It's like if you were to measure something with a ruler,
// you don't measure it according to the X Y and Z of our world, you just want ONE number.
float originDistance = PlaneDistance(vNormal, vTriangle[0]);
// Now the next step is simple, but hard to understand at first. What we need to
// do is get the distance of EACH point from out plane. Above we got the distance of the
// plane to the point (0, 0, 0) which happens to be the origin, now we need to get a distance
// for each point. If the distance is a negative number, then the point is BEHIND the plane.
// If the distance is positive, then the point is in front of the plane. Basically, if the
// line collides with the plane, there should be a negative and positive distance. make sense?
// If the line pierces the plane, it will have a negative distance and a positive distance,
// meaning that a point will be on one side of the plane, and one point on the other. But we
// will do the check after this, first we need to get the distance of each point to the plane.
// Now, we need to use something called the plane equation to get the distance from each point.
// Here is the plane Equation: (Ax + By + Cz + D = The distance from the plane)
// If "The distance from the plane" is 0, that means that the point is ON the plane, which all the polygon points should be.
// A, B and C is the Normal's X Y and Z values. x y and z is the Point's x y and z values.
// "the Point" meaning one of the points of our line. D is the distance that the plane
// is from the origin. We just calculated that and stored it in "originDistance".
// Let's fill in the equation with our data:
// Get the distance from point1 from the plane using: Ax + By + Cz + D = (The distance from the plane)
distance1 = ((vNormal.x * vLine[0].x) + // Ax +
(vNormal.y * vLine[0].y) + // Bx +
(vNormal.z * vLine[0].z)) + originDistance; // Cz + D
// We just got the first distance from the first point to the plane, now let's get the second.
// Get the distance from point2 from the plane using Ax + By + Cz + D = (The distance from the plane)
distance2 = ((vNormal.x * vLine[1].x) + // Ax +
(vNormal.y * vLine[1].y) + // Bx +
(vNormal.z * vLine[1].z)) + originDistance; // Cz + D
// Ok, we should have 2 distances from the plane, from each point of our line.
// Remember what I said about an intersection? If one is negative and one is positive,
// that means that they are both on either side of the plane. So, all we need to do
// is multiply the 2 distances together, and if the result is less than 0, we intersection.
// This works because, any number times a negative number is always negative, IE (-1 * 1 = -1)
// If they are both positive or negative values then it will be above zero.
if(distance1 * distance2 >= 0) // Check to see if both point's distances are both negative or both positive
return false; // Return false if each point has the same sign. -1 and 1 would mean each point is on either side of the plane. -1 -2 or 3 4 wouldn't...
return true; // The line intersected the plane, Return TRUE
}
/*
==================
R_ClipPolygon
==================
*/
bool R_ClipPolygon (int numPoints, vec3_t *points, const cplane_t plane, float epsilon, int *numClipped, vec3_t *clipped){
vec3_t tmpVector, tmpVector2;
float dists[MAX_POLYGON_POINTS];
int sides[MAX_POLYGON_POINTS];
bool frontSide, backSide;
float frac;
int i;
if (numPoints >= MAX_POLYGON_POINTS - 2)
Com_Error(ERR_DROP, "R_ClipPolygon: MAX_POLYGON_POINTS hit");
*numClipped = 0;
// Determine sides for each point
frontSide = false;
backSide = false;
for (i = 0; i < numPoints; i++){
dists[i] = PlaneDistance(plane.normal, plane.dist, points[i]);
if (dists[i] > epsilon){
sides[i] = PLANESIDE_FRONT;
frontSide = true;
continue;
}
if (dists[i] < -epsilon){
sides[i] = PLANESIDE_BACK;
backSide = true;
continue;
}
sides[i] = PLANESIDE_ON;
}
if (!frontSide)
return false; // Not clipped
if (!backSide){
*numClipped = numPoints;
Mem_Copy(clipped, points, numPoints * sizeof(vec3_t));
return true;
}
// Clip it
VectorCopy(points[0], points[i]);
dists[i] = dists[0];
sides[i] = sides[0];
for (i = 0; i < numPoints; i++){
if (sides[i] == PLANESIDE_ON){
VectorCopy(points[i], clipped[(*numClipped)++]);
continue;
}
if (sides[i] == PLANESIDE_FRONT)
VectorCopy(points[i], clipped[(*numClipped)++]);
if (sides[i+1] == PLANESIDE_ON || sides[i+1] == sides[i])
continue;
if (dists[i] == dists[i+1])
VectorCopy(points[i], clipped[(*numClipped)++]);
else {
frac = dists[i] / (dists[i] - dists[i+1]);
VectorSubtract(points[i+1], points[i], tmpVector);
VectorMA(points[i], frac, tmpVector, tmpVector2);
VectorCopy(tmpVector2, clipped[(*numClipped)++]);
}
}
return true;
}
/* Loop over all vertices and determine on which side of the plane
* defined by "n" we live, if not on both sides. We assume at this
* point that the polygon is _FLAT_. The main entry-point
* BSPTreeCreate() has to split polygons such that this is true. Maybe
* we will allow for non-convex polygons here, but _FLAT_ is a must.
*/
static inline PolyPos ClassifyPoly(HPoint3 *plane, Poly *poly,
EdgeIntersection edges[2])
{
HPt3Coord scp0, scp1 = 0.0, scp2, scp3 = 0.0;
PolyPos sign0, sign1 = COPLANAR, sign2, sign3 = COPLANAR;
int i, i0, i2;
scp0 = PlaneDistance(plane, &poly->v[0]->pt);
sign0 = (PolyPos)(fpos(scp0) - fneg(scp0));
if (sign0 == COPLANAR) {
for (i = 1; i < poly->n_vertices; i++) {
scp1 = PlaneDistance(plane, &poly->v[i]->pt);
sign1 = (PolyPos)(fpos(scp1) - fneg(scp1));
if (sign1 != COPLANAR) {
break;
}
scp0 = scp1;
sign0 = sign1;
}
if (i >= 2) {
return sign1;
}
/* at this point: sign0 == 0 and sign1 != 0, loop until we find
* the next zero crossing.
*/
i0 = 0;
sign2 = sign1;
scp2 = scp1;
for (++i; i < poly->n_vertices; i++) {
scp3 = PlaneDistance(plane, &poly->v[i]->pt);
sign3 = (PolyPos)(fpos(scp3) - fneg(scp3));
if (sign3 != sign2) {
break;
}
scp2 = scp3;
sign2 = sign3;
}
if (i == poly->n_vertices) {
return sign1;
}
/* At this point we have sign0 == 0 != sign1 == sign2 != sign3. If
* sign3 == 0, then the next vertex may also be located on the
* plane => sign1 == sign2 determine the side we are located on.
*
* Otherwise sign3 must be != sign1 and we have to sub-divide this
* polygon.
*/
if (sign3 == COPLANAR) {
if (i == poly->n_vertices-1) {
return sign1;
}
scp2 = scp3;
sign2 = sign3;
scp3 = PlaneDistance(plane, &poly->v[++i]->pt);
sign3 = (PolyPos)(fpos(scp3) - fneg(scp3));
if (sign3 == COPLANAR) {
return sign1;
} else if (sign3 == sign1) {
/* impossible case with exact arithmetic; this should mean
* that we are in the COPLANAR case. Assume that? Or retry
* with increased telerance?
*/
return COPLANAR; /* FIXME */
}
}
/* At this point we have sign0 == 0 != sign1. sign2 may be 0, then
* sign3 != sign1. Or 0 == sign0 != sign1 == sign2 != sign 3. At
* any rate we are located on both sides.
*/
i2 = i-1;
} else {
/* Loop until we find a change of sign */
for (i = 1; i < poly->n_vertices; i++) {
scp1 = PlaneDistance(plane, &poly->v[i]->pt);
sign1 = (PolyPos)(fpos(scp1) - fneg(scp1));
if (sign1 != sign0) {
break;
}
scp0 = scp1;
sign0 = sign1;
}
if (i == poly->n_vertices) {
return sign1;
}
i0 = i-1;
sign2 = sign1;
scp2 = scp1;
if (sign2 == COPLANAR) {
/* if sign2 is accidentally 0, then the next point must be != 0,
* or sign0 determines the proper side.
*/
++i;
i %= poly->n_vertices;
scp3 = PlaneDistance(plane, &poly->v[i]->pt);
sign3 = (PolyPos)(fpos(scp3) - fneg(scp3));
if (sign3 == COPLANAR || sign3 == sign0) {
return sign0;
}
scp2 = scp3;
sign2 = sign3;
}
/* loop until we find the next zero crossing, at this point we
* have 0 != sign0 != sign1, 0 != sign2 != sign0, the polygon
* cannot be on one side of the plane.
*/
for (++i; i < poly->n_vertices; i++) {
scp3 = PlaneDistance(plane, &poly->v[i]->pt);
sign3 = (PolyPos)(fpos(scp3) - fneg(scp3));
if (sign3 != sign2) {
break;
}
scp2 = scp3;
sign2 = sign3;
}
if (i == poly->n_vertices) {
if (i0 == 0) {
scp3 = scp0;
sign3 = sign0;
} else {
scp3 = PlaneDistance(plane, &poly->v[0]->pt);
sign3 = (PolyPos)(fpos(scp3) - fneg(scp3));
}
}
i2 = i-1;
}
/* points of intersection between [i0,i0+1], [i2,i2+1] */
edges[0].v[0] = i0;
edges[0].v[1] = i0+1;
edges[0].scp[0] = scp0;
edges[0].scp[1] = scp1;
edges[1].v[0] = i2;
edges[1].v[1] = (i2+1) % poly->n_vertices;
edges[1].scp[0] = scp2;
edges[1].scp[1] = scp3;
return BOTH_SIDES;
}
int CheckCollisionGround( vector<z_face> &collision, vec center, float radius, float angle, float mindist)
{
float ground_skew = (float)cos(angle*PI180)*radius;
vec vpold_vp = mindist*vec(0,-1,0);
for( int i=0; i<collision.size(); i++)
{
vec normal = collision[i].normal;
float distance = PlaneDistance( normal, collision[i].a); // D = - (Ax+By+Cz)
// ---------------------------------------------------------
// najdeme kolidujuci bod na guli
vec ClosestPointOnSphere; // najblizsi bod gule k rovine aktualneho trojuholnika
// najdeme ho ako priesecnik priamky prechadzajucej stredom gule a smerovym vektorom = normalovemu vektoru roviny (trojuholnika)
// vypocitame ho, ale jednodusie tak, ze pripocitame (opocitame) k stredu vektor normala*radius
if( PlanePointDelta( normal, distance, center) > 0 )
{
// center je na strane normaly, blizsi bod je v opacnom smere ako smer normaly
ClosestPointOnSphere = -radius*normal+center;
}
else
{
// center je na opacnej strane ako normala, blizsi bod je v smere normaly
ClosestPointOnSphere = radius*normal+center;
}
// ---------------------------------------------------------
// najdeme kolidujuci bod na trojuholniku
// najprv najdeme kolidujuci bod vzhladom na rovinu v ktorej lezi trojuholnik
vec contactPointSphereToPlane; // kolidujuci bod na rovine trojuholnika s gulou
float distanceCenterToPlane; // vzdialenost stredu gule k rovine
// zistime ci rovina pretina gulu
if( SpherePlaneCollision( center, 0.9999f*radius, normal, distance, &distanceCenterToPlane)==1 )
{
// gula pretina rovinu
// kolidujuci bod je bod na rovine najblizsi k stredu gule
// je vzdialeny od roviny na distanceCenterToPlane, pretoze pocitame bod na rovine pouzijeme -
contactPointSphereToPlane = center-distanceCenterToPlane*normal;
}
else
{
// nie sme v kolizii z gulov, ak sa pohybujeme v smere od roviny, nemoze nastat ziadna kolizia
// ak sa pohybujeme v smere kolmom na normalovy vektor roviny, tak isto kolizia nehrozi
// kvoli nepresnosti vypoctov umoznime pohyb aj ked velmi malou castou smeruje do roviny
// if( DOT3( vpold_vp, center-ClosestPointOnSphere) >= 0)
if( DOT3( vpold_vp, center-ClosestPointOnSphere) > -0.000001f)
{
continue;
}
// gula nepretina rovinu
// kolidujuci bod je priesecnik roviny a priamky vedenej z bodu ClosestPointOnSphere
// v smere pohybu t.j. z vpold do vp
float t = LinePlaneIntersectionDirParameter( ClosestPointOnSphere, vpold_vp, normal, distance);
// t > 1.f, priesecnik z rovinou je dalej ako vpold_vp od bodu ClosestPointOnSphere
if(t>1.f)
continue; // za cely krok vpold_vp sa s tymto trojuholnikom nestretneme
else if( t<-radius/vpold_vp.Length()) // priesecnik je za gulou, v smere pohybu tuto rovinu nestretneme
continue;
else
contactPointSphereToPlane = ClosestPointOnSphere+t*vpold_vp;
}
// najdeme kolidujuci bod na trojuholniku
vec contactPointSphereToTriangle;
// ak sa bod contactPointSphereToPlane nenachadza v trojuholniku
// najdeme najblizsi bod trojuholnika k bodu contactPointSphereToTriangle
if( !PointInsidePolygon( contactPointSphereToPlane, collision[i].a, collision[i].b, collision[i].c) )
{
// najdeme najblizsi bod k contactPointSphereToPlane na hranach trojuholnika
// z tychto vyberieme najblizi k contactPointSphereToPlane
vec closest_ab = ClosestPointOnLine( collision[i].a, collision[i].b, contactPointSphereToPlane);
vec closest_bc = ClosestPointOnLine( collision[i].b, collision[i].c, contactPointSphereToPlane);
vec closest_ca = ClosestPointOnLine( collision[i].c, collision[i].a, contactPointSphereToPlane);
float dist_ab = Distance2( closest_ab, contactPointSphereToPlane);
float dist_bc = Distance2( closest_bc, contactPointSphereToPlane);
float dist_ca = Distance2( closest_ca, contactPointSphereToPlane);
if( dist_ab<dist_bc)
{
if(dist_ab<dist_ca)
contactPointSphereToTriangle = closest_ab;
else
contactPointSphereToTriangle = closest_ca;
}
else
{
if(dist_bc<dist_ca)
contactPointSphereToTriangle = closest_bc;
else
contactPointSphereToTriangle = closest_ca;
}
// kedze kolidujuci bod na trojuholniku je iny ako kolidujuci bod na rovine
// zmeni sa aj kolidujuci bod na guli - ClosestPointOnSphere
// vypocitame ho ako priesecnik gule a priamky z bodu contactPointSphereToTriangle
// v smere pohybu t.j. z vpold do vp
double t1,t2;
if( LineSphereIntersectionDir( contactPointSphereToTriangle, vpold_vp, center, radius, &t1, &t2) )
{
if( t1<=0 && t2<0)
{
// gula je pred trojuholnikom
// berieme bod s t1, lebo ten je blizsie k stene (t1>t2)
if( t1<-1.f)continue; // tento trojuholnik nas nezaujima lebo nekoliduje po cely tento krok
ClosestPointOnSphere = t1*vpold_vp+contactPointSphereToTriangle;
if( (center.y-ClosestPointOnSphere.y)>ground_skew )return 1;
else continue;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
// vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else if( t1>0 && t2<0)
{
// gula je v stene, vratime ju von zo steny
// berieme bod, ktory je blizsie k rovine
vec t1point = t1*vpold_vp+contactPointSphereToTriangle;
vec t2point = t2*vpold_vp+contactPointSphereToTriangle;
/* if(PlanePointDistance( normal, distance, t1point)<=PlanePointDistance( normal, distance, t2point) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
*/
if( ABS(t1) < ABS(t2) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
if( (center.y-ClosestPointOnSphere.y)>ground_skew )return 1;
else continue;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
// vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else // if( t1>0 && t2>0)
{
// gula je za trojuholnikom, gula nekoliduje s trojuholnikom v tomto smere pohybu
continue;
}
}
else
{
// nie je priesecnik, gula je mimo trojuholnika
continue;
}
}
else
{
if( (center.y-ClosestPointOnSphere.y)>ground_skew )return 1;
else continue;
// bod je vnutri trojuholnika
// contactPointSphereToTriangle = contactPointSphereToPlane;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
// vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
/* if( LineSphereIntersectionDir( contactPointSphereToTriangle, vpold_vp, center, radius, &t1, &t2) )
{
if( t1<=0 && t2<0)
{
// gula je pred trojuholnikom
// berieme bod s t1, lebo ten je blizsie k stene (t1>t2)
if( t1<-1.f)continue; // tento trojuholnik nas nezaujima lebo nekoliduje po cely tento krok
return 1;
}
else if( t1>0 && t2<0)
{
return 1; // gula je v stene
}
else // if( t1>0 && t2>0)
{
// gula je za trojuholnikom, gula nekoliduje s trojuholnikom v tomto smere pohybu
continue;
}
}
else
{
// nie je priesecnik, gula je mimo trojuholnika
continue;
}
}
else
{
return 1; // bod je vnutri trojuholnika
}*/
}
return 0;
}
vec CheckCollision( vector<z_face> &collision, vec vp, vec vpold, float radius)
{
// if(vpold_vp.Length()==0)return vpold;
vec vpold_vp = vp-vpold; // smer pohybu
vec vpold_vp_povodny = vpold_vp;// smer pohybu
int iter=0;
float radius2 = radius*radius;
vec newmove = vpold_vp;
vec newClosestPointOnSphere;
vec newContactPointSphereToTriangle;
do
{
float distanceCenterPointOnTriangle=1.e+15f;
int smykanie=0;
for( int i=0; i<collision.size(); i++)
{
vec normal = collision[i].normal;
vec vp_move;
vec center = vpold;
float distance = PlaneDistance( normal, collision[i].a); // D = - (Ax+By+Cz)
// ---------------------------------------------------------
// najdeme kolidujuci bod na guli
vec ClosestPointOnSphere; // najblizsi bod gule k rovine aktualneho trojuholnika
// najdeme ho ako priesecnik priamky prechadzajucej stredom gule a smerovym vektorom = normalovemu vektoru roviny (trojuholnika)
// vypocitame ho, ale jednodusie tak, ze pripocitame (opocitame) k stredu vektor normala*radius
if( PlanePointDelta( normal, distance, center) > 0 )
{
// center je na strane normaly, blizsi bod je v opacnom smere ako smer normaly
ClosestPointOnSphere = -radius*normal+center;
}
else
{
// center je na opacnej strane ako normala, blizsi bod je v smere normaly
ClosestPointOnSphere = radius*normal+center;
}
// ---------------------------------------------------------
// najdeme kolidujuci bod na trojuholniku
// najprv najdeme kolidujuci bod vzhladom na rovinu v ktorej lezi trojuholnik
vec contactPointSphereToPlane; // kolidujuci bod na rovine trojuholnika s gulou
float distanceCenterToPlane; // vzdialenost stredu gule k rovine
// zistime ci rovina pretina gulu
if( SpherePlaneCollision( center, 0.9999f*radius, normal, distance, &distanceCenterToPlane)==1 )
{
// gula pretina rovinu
// kolidujuci bod je bod na rovine najblizsi k stredu gule
// je vzdialeny od roviny na distanceCenterToPlane, pretoze pocitame bod na rovine pouzijeme -
contactPointSphereToPlane = center-distanceCenterToPlane*normal;
}
else
{
// nie sme v kolizii z gulov, ak sa pohybujeme v smere od roviny, nemoze nastat ziadna kolizia
// ak sa pohybujeme v smere kolmom na normalovy vektor roviny, tak isto kolizia nehrozi
// kvoli nepresnosti vypoctov umoznime pohyb aj ked velmi malou castou smeruje do roviny
// if( DOT3( vpold_vp, center-ClosestPointOnSphere) >= 0)
if( DOT3( vpold_vp, center-ClosestPointOnSphere) > -0.000001f)
{
continue;
}
// gula nepretina rovinu
// kolidujuci bod je priesecnik roviny a priamky vedenej z bodu ClosestPointOnSphere
// v smere pohybu t.j. z vpold do vp
float t = LinePlaneIntersectionDirParameter( ClosestPointOnSphere, vpold_vp, normal, distance);
// t > 1.f, priesecnik z rovinou je dalej ako vpold_vp od bodu ClosestPointOnSphere
if(t>1.f)
continue; // za cely krok vpold_vp sa s tymto trojuholnikom nestretneme
else if( t<-radius/vpold_vp.Length()) // priesecnik je za gulou, v smere pohybu tuto rovinu nestretneme
continue;
else
contactPointSphereToPlane = ClosestPointOnSphere+t*vpold_vp;
}
// najdeme kolidujuci bod na trojuholniku
vec contactPointSphereToTriangle;
// ak sa bod contactPointSphereToPlane nenachadza v trojuholniku
// najdeme najblizsi bod trojuholnika k bodu contactPointSphereToTriangle
if( !PointInsidePolygon( contactPointSphereToPlane, collision[i].a, collision[i].b, collision[i].c) )
{
// najdeme najblizsi bod k contactPointSphereToPlane na hranach trojuholnika
// z tychto vyberieme najblizi k contactPointSphereToPlane
vec closest_ab = ClosestPointOnLine( collision[i].a, collision[i].b, contactPointSphereToPlane);
vec closest_bc = ClosestPointOnLine( collision[i].b, collision[i].c, contactPointSphereToPlane);
vec closest_ca = ClosestPointOnLine( collision[i].c, collision[i].a, contactPointSphereToPlane);
float dist_ab = Distance2( closest_ab, contactPointSphereToPlane);
float dist_bc = Distance2( closest_bc, contactPointSphereToPlane);
float dist_ca = Distance2( closest_ca, contactPointSphereToPlane);
if( dist_ab<dist_bc)
{
if(dist_ab<dist_ca)
contactPointSphereToTriangle = closest_ab;
else
contactPointSphereToTriangle = closest_ca;
}
else
{
if(dist_bc<dist_ca)
contactPointSphereToTriangle = closest_bc;
else
contactPointSphereToTriangle = closest_ca;
}
// kedze kolidujuci bod na trojuholniku je iny ako kolidujuci bod na rovine
// zmeni sa aj kolidujuci bod na guli - ClosestPointOnSphere
// vypocitame ho ako priesecnik gule a priamky z bodu contactPointSphereToTriangle
// v smere pohybu t.j. z vpold do vp
double t1,t2;
if( LineSphereIntersectionDir( contactPointSphereToTriangle, vpold_vp, center, radius, &t1, &t2) )
{
if( t1<=0 && t2<0)
{
// gula je pred trojuholnikom
// berieme bod s t1, lebo ten je blizsie k stene (t1>t2)
if( t1<-1.f)continue; // tento trojuholnik nas nezaujima lebo nekoliduje po cely tento krok
ClosestPointOnSphere = t1*vpold_vp+contactPointSphereToTriangle;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else if( t1>0 && t2<0)
{
// gula je v stene, vratime ju von zo steny
// berieme bod, ktory je blizsie k rovine
vec t1point = t1*vpold_vp+contactPointSphereToTriangle;
vec t2point = t2*vpold_vp+contactPointSphereToTriangle;
/* if(PlanePointDistance( normal, distance, t1point)<=PlanePointDistance( normal, distance, t2point) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
*/
if( ABS(t1) < ABS(t2) )
ClosestPointOnSphere = t1point;
else
ClosestPointOnSphere = t2point;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
else // if( t1>0 && t2>0)
{
// gula je za trojuholnikom, gula nekoliduje s trojuholnikom v tomto smere pohybu
continue;
}
}
else
{
// nie je priesecnik, gula je mimo trojuholnika
continue;
}
}
else
{
// bod je vnutri trojuholnika
contactPointSphereToTriangle = contactPointSphereToPlane;
// mozeme sa pohnut iba tolko pokial sa colidujuci bod na guli nedotkne
// kolidujuceho bodu na trojuholniku
vp_move = contactPointSphereToTriangle - ClosestPointOnSphere;
}
// zistime vzdialenost kontaktneho bodu na trojuholniku ku stredu gule
float dist = Distance2(contactPointSphereToTriangle,center);
if(dist<radius2) // ak je mensi ako polomer, gula je v kolizii z polygonom
{
if(dist<distanceCenterPointOnTriangle) // ak vzdialenost je mensia ako ineho bodu v kolizii, nahradime ho
{
distanceCenterPointOnTriangle=dist;
newmove = vp_move;
newClosestPointOnSphere = ClosestPointOnSphere;
newContactPointSphereToTriangle = contactPointSphereToTriangle;
}
}
else
{
if(distanceCenterPointOnTriangle>5.e+14f) // nenasiel sa ziaden bod vnutri gule
{
if( vp_move.Length2() < newmove.Length2() ) // berieme kratsi
{
newmove = vp_move;
newClosestPointOnSphere = ClosestPointOnSphere;
newContactPointSphereToTriangle = contactPointSphereToTriangle;
}
}
}
smykanie=1;
}
if(smykanie)
{
vec normal=vpold-newClosestPointOnSphere;
float distance = PlaneDistance( normal, newContactPointSphereToTriangle);
vec delta = LinePlaneIntersectionDir( newClosestPointOnSphere+vpold_vp, normal, normal, distance)-newContactPointSphereToTriangle;
// vec newvp = newClosestPointOnSphere+vpold_vp;
// float distancepoint = PlanePointDelta( normal, distance, newvp);
// vec intersec = -distancepoint*normal+newvp;
// vec delta = intersec-newContactPointSphereToTriangle;
// taky klzavy pohyb, ktory ide proti povodnemu pohybu zamietneme
if( DOT3(vpold_vp_povodny, delta) < 0)delta.clear();
vpold += newmove; // posunieme sa po najblizi kolidujuci bod
vpold += 0.000001f*normal;
vp = vpold + delta; // cielovy bod posunieme o deltu klzanim
vpold_vp = vp-vpold; // novy vektor pohybu
newmove = vpold_vp;
iter++;
}
else
{
vpold += newmove;
vpold_vp.clear();
iter=1000;
}
}
while( (vpold_vp.Length2()>1.e-8f)&&(iter<10) );
return vpold;
}
