1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69
| bool AInfrastructureGraphManager::DichotomyStep(FVector a0, FVector a1, FVector a2, FVector b0, FVector b1, FVector b2) {
if (CollisionSegSeg(GetLocationAtDistanceAlongSpline(a0, a1, a2, MinS), GetLocationAtDistanceAlongSpline(a0, a1, a2, (MinS + MaxS) / 2), GetLocationAtDistanceAlongSpline(b0, b1, b2, MinT), GetLocationAtDistanceAlongSpline(b0, b1, b2, (MinT + MaxT) / 2)) == true) {
MaxS = (MinS + MaxS) / 2;
MaxT = (MinT + MaxT) / 2;
return true;
}
if (CollisionSegSeg(GetLocationAtDistanceAlongSpline(a0, a1, a2, (MinS + MaxS) / 2), GetLocationAtDistanceAlongSpline(a0, a1, a2, MaxS), GetLocationAtDistanceAlongSpline(b0, b1, b2, MinT), GetLocationAtDistanceAlongSpline(b0, b1, b2, (MinT + MaxT) / 2)) == true) {
MinS = (MinS + MaxS) / 2;
MaxT = (MinT + MaxT) / 2;
return true;
}
if (CollisionSegSeg(GetLocationAtDistanceAlongSpline(a0, a1, a2, MinS), GetLocationAtDistanceAlongSpline(a0, a1, a2, (MinS + MaxS) / 2), GetLocationAtDistanceAlongSpline(b0, b1, b2, (MinT + MaxT) / 2), GetLocationAtDistanceAlongSpline(b0, b1, b2, MaxT)) == true) {
MaxS = (MinS + MaxS) / 2;
MinT = (MinT + MaxT) / 2;
return true;
}
if (CollisionSegSeg(GetLocationAtDistanceAlongSpline(a0, a1, a2, (MinS + MaxS) / 2), GetLocationAtDistanceAlongSpline(a0, a1, a2, MaxS), GetLocationAtDistanceAlongSpline(b0, b1, b2, (MinT + MaxT) / 2), GetLocationAtDistanceAlongSpline(b0, b1, b2, MaxT)) == true) {
MinS = (MinS + MaxS) / 2;
MinT = (MinT + MaxT) / 2;
return true;
}
return false;
}
FVector AInfrastructureGraphManager::GetLocationAtDistanceAlongSpline(FVector PointA, FVector PointB, FVector PointC, float s) {
float l_X = (1 - s)*(1 - s)*PointA.X + 2 * s*(1 - s)*PointC.X + s*s*PointB.X;
float l_Y = (1 - s)*(1 - s)*PointA.Y + 2 * s*(1 - s)*PointC.Y + s*s*PointB.Y;
return FVector(l_X, l_Y, 0.f);
}
FVector AInfrastructureGraphManager::GetSplinesIntersection(FVector a0, FVector a1, FVector a2, FVector b0, FVector b1, FVector b2) {
float l_epsilon = 0.01; //plus il est petit plus c'est précis mais couteux et inversement.
//TODO à optimiser et rendre proportionnel à la longueur de la spline la plus longue
bool l_bool = true;
MinS = 0; MaxS = 1; MinT = 0; MaxT = 1;
while (MaxS - MinS > l_epsilon && l_bool == true) {
l_bool = DichotomyStep(a0, a1, a2, b0, b1, b2);
}
return GetLocationAtDistanceAlongSpline(a0, a1, a2, (MinS + MaxS) / 2.0);
} |
Partager