public float Angle(Vector3 p0, Vector3 p1, Vector3 p2)
{
float rtn;
Vector2 Pt0_2d = VectGeom.Convert2d(p0);
Vector2 Pt1_2d = VectGeom.Convert2d(p1);
Vector2 Pt2_2d = VectGeom.Convert2d(p2);
rtn = Vector2.Angle(Pt0_2d - Pt1_2d, Pt2_2d - Pt1_2d);
//Work out the sign of the angle b doing a cross product
Vector3 cross = Vector3.Cross(Pt0_2d - Pt1_2d, Pt2_2d - Pt1_2d);
if (cross.z > 0)
{
rtn = -rtn;
}
return(rtn);
}
public float Angle(int CtrlPtIdx)
{
float rtn;
if (CtrlPtIdx < 2)
{
return(180);
}
Vector2 Pt0_2d = VectGeom.Convert2d(CtrlPts[CtrlPtIdx - 1].Pos);
Vector2 Pt1_2d = VectGeom.Convert2d(CtrlPts[CtrlPtIdx].Pos);
Vector2 Pt2_2d = VectGeom.Convert2d(CtrlPts[CtrlPtIdx + 1].Pos);
rtn = Vector2.Angle(Pt0_2d - Pt1_2d, Pt2_2d - Pt1_2d);
//Work out the sign of the angle b doing a cross product
Vector3 cross = Vector3.Cross(Pt0_2d - Pt1_2d, Pt2_2d - Pt1_2d);
if (cross.z > 0)
{
rtn = -rtn;
}
return(rtn);
}
internal void CalcTurnin()
{
//GameObject goCirc;
//CapsuleCollider Circ;
//goCirc = GameObject.CreatePrimitive(PrimitiveType.Cube);
//goCirc.GetComponent<BoxCollider>().enabled = false;
//Circ = goCirc.AddComponent<CapsuleCollider>();
//goCirc.name = "CircColl" + BendId;
//Circ.height = 100;
Radius = SqrtRad * SqrtRad;
Vector3 Centre = new Vector3(0, 0, 0);
Vector3 C = new Vector3(0, 0, 0);
Vector3 AP = VectGeom.Convert2d(ApexPos);
Vector3 TP = new Vector3(0, 0, 0);
Vector3 EP = new Vector3(0, 0, 0);
int e = 0;
float ExitFrac = 1;//1 is a wide turn, 0 is tight
float SmallestRadErr = 1000;
PossBendCircle bbc = new PossBendCircle();
Vector3 ApexPos2d = VectGeom.Convert2d(ApexPos);
float NxtBndDist = Rd.XSecs.Diff(ApexXSec, NextBend.ApexXSec);
//Method:
//Try a range of turnin segs
//For each Turnin seg, draw a line perpendicular to the fwd direction
//and find the turninPt (TP)
//Find the midpoint (MP) of the line from apex to turninPoint and draw a line perpendicular to this (MPerp)
//Where MPerp crosses TPerp is the centre of the circle, C
/// <image url="$(SolutionDir)\CommonImages\CalcTurninAlgorithm.png" scale="1.2"/>
if (Type == BendType.Right)
{
if (NextBend.Type == BendType.Left && NxtBndDist < 200)
{
ExitFrac = NxtBndDist / 200;
}
for (XSec tx = Rd.XSecs[StartSegIdx - 70]; tx.IsBefore(Rd.XSecs[StartSegIdx]); tx = Rd.XSecs.Next(tx)) //removed circlebug
{
Vector2 TPerp = VectGeom.Convert2d(Vector3.Cross(Vector3.up, tx.Forward));
TP = VectGeom.Convert2d(tx.KerbL + (tx.KerbR - tx.KerbL).normalized * TurninGap);
Vector2 MP = (ApexPos2d + TP) / 2;
Vector2 MPerp = new Vector2((ApexPos2d - TP).y, -(ApexPos2d - TP).x);
if (VectGeom.LineLineIntersection(out C, TP, TPerp, MP, MPerp)) //these vars are all Vector3(x,0,y)
{
float biggestCos = 0;
float R = Vector2.Distance(TP, C);
PossBendCircle pbc = new PossBendCircle();
float RadErr = 1000;
for (e = EndXSec.Idx + 70; e > EndXSec.Idx; e--) //bugbugbug circlebug
{
Vector2 WideExitPt = VectGeom.Convert2d(Rd.XSecs[e].KerbL + (Rd.XSecs[e].KerbR - Rd.XSecs[e].KerbL).normalized * 2);
Vector2 TightExitPoint = VectGeom.Convert2d(Rd.XSecs[e].KerbR + (Rd.XSecs[e].KerbL - Rd.XSecs[e].KerbR).normalized * 2);
EP = Vector2.Lerp(TightExitPoint, WideExitPt, ExitFrac);
Vector3 EPerp = VectGeom.Convert2d(-Rd.XSecs[e].Right).normalized;
float cos = Mathf.Abs(Vector2.Dot((EP - C).normalized, EPerp));
if (cos > biggestCos)
{
RadErr = Mathf.Abs(Vector3.Distance(EP, C) - R);
pbc = new PossBendCircle {
C = C, RSq = 0, RadErr = RadErr, EP = EP, EX = Rd.XSecs[e], TP = TP, TX = tx
};
biggestCos = cos;
}
}
if (RadErr < SmallestRadErr)
{
bbc = new PossBendCircle {
C = pbc.C, RSq = 0, RadErr = pbc.RadErr, EP = pbc.EP, EX = pbc.EX, TP = pbc.TP, TX = pbc.TX
};
SmallestRadErr = pbc.RadErr;
}
}
}
Centre = new Vector3(bbc.C.x, ApexPos.y, bbc.C.y);
Radius = Vector2.Distance(bbc.TP, bbc.C);
TurninXSec = bbc.TX;
TurninSegIdx = TurninXSec.Idx;
TurninPos = new Vector3(bbc.TP.x, ApexPos.y, bbc.TP.y);
ExitXSec = bbc.EX;
ExitPos = new Vector3(bbc.EP.x, ApexPos.y, bbc.EP.y);
goto FoundCentre;
//Not found Centre
Debug.Log("No Centre for Bend" + BendId);
ExitXSec = Rd.XSecs[e];
Debug.Break();
}
if (Type == BendType.Left)
{
if (NextBend.Type == BendType.Right && NxtBndDist < 200)
{
ExitFrac = NxtBndDist / 200;
}
for (int t = StartSegIdx - 70; t < StartSegIdx; t++) //bugbugbug circlebug
{
Vector2 TPerp = VectGeom.Convert2d(Vector3.Cross(Vector3.up, Rd.XSecs[t].Forward));
TP = VectGeom.Convert2d(Rd.XSecs[t].KerbR + (Rd.XSecs[t].KerbL - Rd.XSecs[t].KerbR).normalized * TurninGap);
Vector2 MP = (ApexPos2d + TP) / 2;
Vector2 MPerp = new Vector2((ApexPos2d - TP).y, -(ApexPos2d - TP).x);
if (VectGeom.LineLineIntersection(out C, TP, TPerp, MP, MPerp)) //these vars are all Vector3(x,0,y)
{
float biggestCos = 0;
float R = Vector2.Distance(TP, C);
PossBendCircle pbc = new PossBendCircle();
float RadErr = 1000;
for (e = EndXSec.Idx + 70; e > EndXSec.Idx; e--) //bugbugbug circlebug
{
Vector2 WideExitPt = VectGeom.Convert2d(Rd.XSecs[e].KerbR + (Rd.XSecs[e].KerbL - Rd.XSecs[e].KerbR).normalized * 2);
Vector2 TightExitPoint = VectGeom.Convert2d(Rd.XSecs[e].KerbL + (Rd.XSecs[e].KerbR - Rd.XSecs[e].KerbL).normalized * 2);
EP = Vector2.Lerp(TightExitPoint, WideExitPt, ExitFrac);
Vector3 EPerp = VectGeom.Convert2d(Rd.XSecs[e].Right).normalized;
float cos = Mathf.Abs(Vector2.Dot((EP - C).normalized, EPerp));
if (cos > biggestCos)
{
RadErr = Mathf.Abs(Vector3.Distance(EP, C) - R);
pbc = new PossBendCircle {
C = C, RSq = 0, RadErr = RadErr, EP = EP, EX = Rd.XSecs[e], TP = TP, TX = Rd.XSecs[t]
};
biggestCos = cos;
}
}
if (RadErr < SmallestRadErr)
{
bbc = new PossBendCircle {
C = pbc.C, RSq = 0, RadErr = pbc.RadErr, EP = pbc.EP, EX = pbc.EX, TP = pbc.TP, TX = pbc.TX
};
SmallestRadErr = pbc.RadErr;
}
}
}
Centre = new Vector3(bbc.C.x, ApexPos.y, bbc.C.y);
Radius = Vector2.Distance(bbc.TP, bbc.C);
TurninXSec = bbc.TX;
TurninSegIdx = TurninXSec.Idx;
TurninPos = new Vector3(bbc.TP.x, ApexPos.y, bbc.TP.y);
ExitXSec = bbc.EX;
ExitPos = new Vector3(bbc.EP.x, ApexPos.y, bbc.EP.y);
goto FoundCentre;
//Not found Centre
Debug.Log("No Centre for Bend" + BendId);
ExitXSec = Rd.XSecs[e];
Debug.Break();
}
FoundCentre:
//goCirc.transform.position = Centre;
//Circ.radius = Radius;
Angle = VectGeom.SignedAngle(TurninXSec.Forward, ExitXSec.Forward);
float RacelineSegCount = Rd.XSecs.Diff(TurninXSec, ExitXSec);;
RacelineAnglePerSeg = Angle / RacelineSegCount;
RacelineSegLength = Angle * Mathf.Deg2Rad * Radius / RacelineSegCount;
CalculateSpeed();
//Circ.enabled = false; //cos the gameobject doesnt get destroyed till end of frame
//GameObject.Destroy(goCirc);
return;
NoTurnin:
if (TurninXSec == null)
{
TurninXSec = Rd.XSecs[ApexSegIdx - 50]; TurninSegIdx = TurninXSec.Idx;
ExitXSec = Rd.XSecs[ApexSegIdx + (ApexSegIdx - TurninSegIdx)];
if (Type == BendType.Right)
{
TurninPos = TurninXSec.KerbL + (TurninXSec.KerbR - TurninXSec.KerbL).normalized * 4f;
}
else
{
TurninPos = TurninXSec.KerbR + (TurninXSec.KerbL - TurninXSec.KerbR).normalized * 4f;
}
Radius = 100;
AnalyseTurnin();
CalculateSpeed();
}
//Circ.enabled = false; //cos the gameobject doesnt get destroyed till end of frame
//GameObject.Destroy(goCirc);
}
