private void UpdateOmniPosition(TrajectoryData trajectoryInfoX, TrajectoryData trajectoryInfoY)
{
Vector3 goalPosition = new Vector3();
goalPosition.x = trajectoryInfoX.GoalPositions[this.targetPointIndex];
goalPosition.y = trajectoryInfoY.GoalPositions[this.targetPointIndex];
Vector3 newPosition = Vector3.Lerp(this.startPosition, goalPosition, this.progressTimeRatio);
Vector3 deltaPosition = newPosition - this.GetCurrentRosPosition();
float deltaLinearSpeed = Mathf.Sqrt(Mathf.Pow(deltaPosition.x, 2) + Mathf.Pow(deltaPosition.y, 2));
if (deltaLinearSpeed > HSRCommon.MaxSpeedBase * Time.fixedDeltaTime)
{
float deltaLinearSpeedClamped = Mathf.Clamp(deltaLinearSpeed, 0.0f, HSRCommon.MaxSpeedBase * Time.fixedDeltaTime);
deltaPosition.x = deltaPosition.x * deltaLinearSpeedClamped / deltaLinearSpeed;
deltaPosition.y = deltaPosition.y * deltaLinearSpeedClamped / deltaLinearSpeed;
}
Vector3 deltaNoisePos = new Vector3();
deltaNoisePos.x = this.GetPosNoise(deltaPosition.x);
deltaNoisePos.y = this.GetPosNoise(deltaPosition.y);
//update position
this.baseFootprintRigidbody.localPosition += GetUnityPositionFromRosPosition(deltaPosition);
this.baseFootprintPosNoise.localPosition += GetUnityPositionFromRosPosition(deltaNoisePos);
}
private void UpdateOmniRotation(TrajectoryData trajectoryInfo)
{
float newRotation = Mathf.LerpAngle(this.startRotation * Mathf.Rad2Deg, trajectoryInfo.GoalPositions[this.targetPointIndex] * Mathf.Rad2Deg, this.progressTimeRatio) * Mathf.Deg2Rad;
float deltaAngle = newRotation - this.GetCurrentRosRotation();
if (deltaAngle < -Math.PI)
{
deltaAngle += (float)(2 * Math.PI);
}
if (deltaAngle > Math.PI)
{
deltaAngle -= (float)(2 * Math.PI);
}
if (Math.Abs(deltaAngle) > HSRCommon.MaxSpeedBaseRad * Time.fixedDeltaTime)
{
float maxSpeedBaseRadAtFixedDeltaTime = HSRCommon.MaxSpeedBaseRad * Time.fixedDeltaTime;
deltaAngle = Mathf.Clamp(deltaAngle, -maxSpeedBaseRadAtFixedDeltaTime, +maxSpeedBaseRadAtFixedDeltaTime);
}
Quaternion deltaRotQua = Quaternion.Euler(new Vector3(0.0f, 0.0f, -deltaAngle * Mathf.Rad2Deg));
Quaternion deltaNoiseRotQua = Quaternion.Euler(new Vector3(0.0f, 0.0f, -this.GetRotNoise(deltaAngle * Mathf.Rad2Deg)));
//update rotation.
this.baseFootprintRigidbody.localRotation *= deltaRotQua;
this.baseFootprintRotNoise.localRotation *= deltaNoiseRotQua;
}
public void CirclesXY()
{
// Coarse theta stepping, so only two matches
Debug.Log(System.Reflection.MethodBase.GetCurrentMethod().Name);
float dtheta = 0.05f;
float radius = 10f;
float offset = radius / 3f;
TrajectoryData c1 = new TrajectoryData();
TrajectoryData c2 = new TrajectoryData();
Debug.Log(string.Format("r={0} offset={1} dtheta={2}", radius, offset, dtheta));
for (float theta = 0f; theta < 2f * Mathf.PI; theta += dtheta)
{
Vector3 r = new Vector3(radius * Mathf.Cos(theta) + offset, radius * Mathf.Sin(theta), 0);
c1.AddPoint(r, Vector3.zero, theta);
r = new Vector3(radius * Mathf.Cos(theta) - offset, radius * Mathf.Sin(theta), 0);
c2.AddPoint(r, Vector3.zero, theta);
}
// check the lines intersect at the origin
List <TrajectoryData.Intercept> intercepts = new List <TrajectoryData.Intercept>();
intercepts = c1.GetIntercepts(c2, 0.3f, 1f);
Debug.Log("Num intercepts=" + intercepts.Count);
foreach (TrajectoryData.Intercept intercept in intercepts)
{
Debug.Log(string.Format("r_i={0} r_j={1}", intercept.tp1.r, intercept.tp2.r));
}
Assert.IsTrue(intercepts.Count == 2);
}
public void LineIntersection2DXY()
{
Debug.Log(System.Reflection.MethodBase.GetCurrentMethod().Name);
float slope1 = 2f;
float slope2 = 0.5f;
TrajectoryData line1 = new TrajectoryData();
TrajectoryData line2 = new TrajectoryData();
for (float x = -20f; x < 20f; x++)
{
Vector3 r = new Vector3(x, slope1 * x, 0);
line1.AddPoint(r, Vector3.zero, x);
r = new Vector3(x, slope2 * x, 0);
line2.AddPoint(r, Vector3.zero, x);
}
// debug
// Debug.Log("points=" + line1.Count());
// for (int i=0; i < line1.Count(); i++) {
// TrajectoryData.Tpoint tp = line1.GetByIndex(i);
// Debug.Log(string.Format("{0} r={1}", i, tp.r));
// }
// check the lines intersect at the origin
List <TrajectoryData.Intercept> intercepts = new List <TrajectoryData.Intercept>();
intercepts = line1.GetIntercepts(line2, 0.1f, 1f);
Assert.IsTrue(intercepts.Count == 1);
}
public void CirclesYZManyIntercepts()
{
// Fine grained theta. intersection yields many points, bust should be filtered down
// to the two best
Debug.Log(System.Reflection.MethodBase.GetCurrentMethod().Name);
float dtheta = 0.01f;
float radius = 10f;
float offset = radius / 3f;
TrajectoryData c1 = new TrajectoryData();
TrajectoryData c2 = new TrajectoryData();
Debug.Log(string.Format("r={0} offset={1} dtheta={2}", radius, offset, dtheta));
for (float theta = 0f; theta < 2f * Mathf.PI; theta += dtheta)
{
Vector3 r = new Vector3(0, radius * Mathf.Cos(theta) + offset, radius * Mathf.Sin(theta));
c1.AddPoint(r, Vector3.zero, theta);
r = new Vector3(0, radius * Mathf.Cos(theta) - offset, radius * Mathf.Sin(theta));
c2.AddPoint(r, Vector3.zero, theta);
}
// check the lines intersect at the origin
List <TrajectoryData.Intercept> intercepts = new List <TrajectoryData.Intercept>();
intercepts = c1.GetIntercepts(c2, 0.2f, 1f);
Debug.Log("Num intercepts=" + intercepts.Count);
foreach (TrajectoryData.Intercept intercept in intercepts)
{
Debug.Log(string.Format("r_i={0} r_j={1}", intercept.tp1.r, intercept.tp2.r));
}
Assert.IsTrue(intercepts.Count == 2);
}
private void Shoot()
{
int lRtn;
double lDR = 0; //Trajectory Range increment.
double lCR; //Current Range.
TrajectoryData lTD;
lRtn = _MyBallisticsCalculator.PreflightCheck();
if (lRtn < 0)
{
PostFlightCheck(lRtn);
return;
}
lDR = 1;
lCR = lDR;
_MyTrajectories = new ObservableCollection <TrajectoryData>();
while (lCR < _MyBallisticsCalculator.ShotDistance)
{
lTD = new TrajectoryData();
lTD.Range = lCR;
lTD.MuzzleDrop = MyBallisticsCalculator.MuzzleDrop(lCR);
lTD.SightDelta = MyBallisticsCalculator.SightDelta(lCR);
lTD.Velocity = MyBallisticsCalculator.Velocity(lCR);
lTD.Energy = MyBallisticsCalculator.Energy(MyBallisticsCalculator.BulletWeight, lCR);
lTD.SpinRate = MyBallisticsCalculator.SpinRate(lCR);
lTD.GyroStability = MyBallisticsCalculator.GyroscopicStability(MyBallisticsCalculator.Velocity(lCR), MyBallisticsCalculator.TempF, MyBallisticsCalculator.BaroPressure);
lTD.HorizDev = MyBallisticsCalculator.TotalHorizontalDrift(lCR);
lTD.CoriolisH = MyBallisticsCalculator.GetCoriolisHoriz(lCR);
lTD.CoriolisV = MyBallisticsCalculator.GetCoriolisVert(lCR);
lTD.SpinDrift = MyBallisticsCalculator.GetSpinDrift(lCR);
lTD.WindDeflect = MyBallisticsCalculator.WindDriftDegrees(_MyBallisticsCalculator.WindSpeed, _MyBallisticsCalculator.WindDirectionDeg, lCR);
lTD.FlightTime = MyBallisticsCalculator.FlightTime(lCR);
lCR += lDR;
if (lCR >= _MyBallisticsCalculator.ShotDistance)
{
lTD = new TrajectoryData();
lTD.Range = _MyBallisticsCalculator.ShotDistance;
lTD.MuzzleDrop = MyBallisticsCalculator.MuzzleDrop(_MyBallisticsCalculator.ShotDistance);
lTD.SightDelta = MyBallisticsCalculator.SightDelta(_MyBallisticsCalculator.ShotDistance);
lTD.Velocity = MyBallisticsCalculator.Velocity(_MyBallisticsCalculator.ShotDistance);
lTD.Energy = MyBallisticsCalculator.Energy(MyBallisticsCalculator.BulletWeight, _MyBallisticsCalculator.ShotDistance);
lTD.SpinRate = MyBallisticsCalculator.SpinRate(lCR);
lTD.GyroStability = MyBallisticsCalculator.GyroscopicStability(MyBallisticsCalculator.Velocity(_MyBallisticsCalculator.ShotDistance), MyBallisticsCalculator.TempF, MyBallisticsCalculator.BaroPressure);
lTD.HorizDev = MyBallisticsCalculator.TotalHorizontalDrift(_MyBallisticsCalculator.ShotDistance);
lTD.CoriolisH = MyBallisticsCalculator.GetCoriolisHoriz(_MyBallisticsCalculator.ShotDistance);
lTD.CoriolisV = MyBallisticsCalculator.GetCoriolisVert(_MyBallisticsCalculator.ShotDistance);
lTD.SpinDrift = MyBallisticsCalculator.GetSpinDrift(_MyBallisticsCalculator.ShotDistance);
lTD.WindDeflect = MyBallisticsCalculator.WindDriftDegrees(_MyBallisticsCalculator.WindSpeed, _MyBallisticsCalculator.WindDirectionDeg, _MyBallisticsCalculator.ShotDistance);
lTD.FlightTime = MyBallisticsCalculator.FlightTime(_MyBallisticsCalculator.ShotDistance);
}
_MyTrajectories.Add(lTD);
}
RaisePropertyChanged(nameof(MyTrajectories));
LoadCharts();
}
public void Init(float worldTime)
{
Cleanup();
lineRenderer = GetComponent <LineRenderer>();
points = new List <TrajPoint>();
lastPoint = new Vector3(float.MaxValue, 0, 0);
lineRenderer.positionCount = 2;
lineRenderer.SetPosition(0, Vector3.zero);
lineRenderer.SetPosition(1, Vector3.up);
timeMarks = new List <TimeMark>();
lastTimeMark = worldTime;
trajectoryData = new TrajectoryData();
}
private void UpdateProgressTimeRatio()
{
TrajectoryData trajectoryInfo = this.trajectoryInfoMap[HSRCommon.Joint.odom_x];
if (this.targetPointIndex == 0)
{
this.progressTimeRatio = (Time.time - trajectoryInfo.StartTime) / (trajectoryInfo.Durations[this.targetPointIndex]);
}
else
{
this.progressTimeRatio = (Time.time - (trajectoryInfo.StartTime + trajectoryInfo.Durations[this.targetPointIndex - 1])) / (trajectoryInfo.Durations[this.targetPointIndex] - trajectoryInfo.Durations[this.targetPointIndex - 1]);
}
}
/// <summary>
/// Compare this trajectory data set to another and find those points that are within
/// the specified deltaDistance. DeltaDistance denotes the seperation in EACH
/// co-ordinate (i.e. they are within in a BOX of size delta distance) to reduce CPU
/// cost of calculating exact distance.)
///
/// DeltaTime specifies the time within which multiple intercept points should be regarded
/// as duplicates (in which case the intercept with the closest approach is used)
///
/// List of intercepts is provided in time order (earliest first)
/// </summary>
/// <param name="tdata"></param>
/// <param name="deltaDistance"></param>
/// <param name="deltaTime"></param>
/// <returns></returns>
public List <Intercept> GetIntercepts(TrajectoryData tdata, float deltaDistance, float deltaTime)
{
List <Intercept> intercepts = new List <Intercept>();
tpoints.Sort(new TpointCompare());
tdata.Sort();
// Concept: Lists are ordered so can walk each
int i = 0;
int j = 0;
while ((i < tpoints.Count) && (j < tdata.Count()))
{
Tpoint tp_i = (Tpoint)tpoints[i];
Tpoint tp_j = (Tpoint)tdata.GetByIndex(j);
// Debug.Log(string.Format("i={0} j={1} r_i={2} r_j={3}", i, j, tp_i.r, tp_j.r));
if (Mathf.Abs(tp_i.r.x - tp_j.r.x) < deltaDistance)
{
if (Mathf.Abs(tp_i.r.y - tp_j.r.y) < deltaDistance)
{
if (Mathf.Abs(tp_i.r.z - tp_j.r.z) < deltaDistance)
{
Intercept intercept = new Intercept();
intercept.tp1 = tp_i;
intercept.tp2 = tp_j;
intercept.dR = Vector3.Distance(tp_i.r, tp_j.r);
intercept.dV = Vector3.Distance(tp_i.v, tp_j.v);
intercept.dT = tp_i.t - tp_j.t;
intercept.tp1_index = i;
intercept.tp2_index = j;
intercepts.Add(intercept);
i++;
j++;
continue;
}
}
}
if (comparer.Compare(tp_i, tp_j) > 0)
{
j++;
}
else
{
i++;
}
}
List <Intercept> uniqueIntercepts = RemoveDuplicates(intercepts, deltaDistance, deltaTime);
// sort
uniqueIntercepts.Sort(interceptComparer);
return(uniqueIntercepts);
}
private void UpdateTargetPointIndex()
{
TrajectoryData trajectoryInfo = this.trajectoryInfoMap[HSRCommon.Joint.odom_x];
int tempIndex = 0;
for (int i = 0; i < trajectoryInfo.Durations.Count; i++)
{
tempIndex = i;
if (Time.time - trajectoryInfo.StartTime < trajectoryInfo.Durations[tempIndex])
{
break;
}
}
this.targetPointIndex = tempIndex;
}
public void Basic()
{
TrajectoryData tdata = new TrajectoryData();
for (float x = 0; x < 10f; x++)
{
Vector3 r = new Vector3(x, 0, 0);
tdata.AddPoint(r, Vector3.zero, x);
}
// check each entry is less than next
for (int i = 0; i < tdata.Count() - 1; i++)
{
TrajectoryData.Tpoint tp1 = tdata.GetByIndex(i);
TrajectoryData.Tpoint tp2 = tdata.GetByIndex(i + 1);
Assert.IsTrue(tp1.r.x < tp2.r.x);
}
}
public static void TrajectoryPath(ref Vector3[] points, Vector3 start, Vector3 target, float gravity)
{
TrajectoryData launchData = CalculateTrajectoryData(start, target, gravity);
//Vector3 previousDrawPoint = start;
//points = new Vector3[resolution];
//points[0] = start;
//int resolution = 30;
for (int i = 0; i < points.Length; i++)
{
float simulationTime = i / (float)points.Length * launchData.timeToTarget;
Vector3 displacement = launchData.initialVelocity * simulationTime + Vector3.up * gravity * simulationTime * simulationTime / 2f;
Vector3 drawPoint = start + displacement;
points[i] = drawPoint;
//Debug.DrawLine(previousDrawPoint, drawPoint, Color.green);
//previousDrawPoint = drawPoint;
}
}
public void LineIntersection2DZY()
{
Debug.Log(System.Reflection.MethodBase.GetCurrentMethod().Name);
float slope1 = 2f;
float slope2 = 0.5f;
TrajectoryData line1 = new TrajectoryData();
TrajectoryData line2 = new TrajectoryData();
for (float x = -20f; x < 20f; x++)
{
Vector3 r = new Vector3(0, slope1 * x, x);
line1.AddPoint(r, Vector3.zero, x);
r = new Vector3(0, slope2 * x, x);
line2.AddPoint(r, Vector3.zero, x);
}
// check the lines intersect at the origin
List <TrajectoryData.Intercept> intercepts = new List <TrajectoryData.Intercept>();
intercepts = line1.GetIntercepts(line2, 0.1f, 1f);
Assert.IsTrue(intercepts.Count == 1);
}
/// <summary>
/// Caclulate transition tranjectory from point p0 to point p1 around attractor f0.
/// </summary>
/// <param name="p0">First position vector.</param>
/// <param name="p1">Second position vector.</param>
/// <param name="f0">Attractor position vector.</param>
/// <param name="hyperbola">Trajectory hyperbola.</param>
/// <param name="targetDuration">Preferred duration.</param>
/// <param name="isReverseOrbit">Is transfer orbit plane flipped.</param>
/// <param name="attrMass">Attractor mass.</param>
/// <param name="g">Gravity constant.</param>
/// <param name="precision">Calculation precision.</param>
/// <param name="semiMajorAxisUpperLimit">Transition ellipse semi major axis limit.</param>
/// <returns>Calculated trajectory data.</returns>
/// <remarks>
/// Main task of this component is to find elliptic trajectory between any 2 points, orbiting around single attractor.
/// The core problem can be described in more formal form:
/// let's take a body A and body B; this two bodies rotating around attractor F. The Goal is to find an orbit around attractor F, which passes through vector A and B.
/// This problem is equivalent to to problem of finding of an ellipse when only one focus and two points on ellipse are given,
/// which is known as Lambert's problem (but only with one allowed revolution around attractor). The answer to this problem actually is quite simple. In simplified form the solution can be described in these steps:
/// 1. Build a hyperbola, using given points A and B as focuses of hyperbola, and attractor F as point on one branch of hyperbola.
/// 2. Place a new point F2 anywhere on branch opposite to main branch of hyperbola ( where main branch is branch, on which F is located)
/// 3. Build an ellipse using 3 points: F as first focus, new point F2 as second focus, A or B as one point on ellipse.
/// 4. As result, created ellipse will always exactly correspond to transition orbit, and it always will pass through A and B.
/// Eccentricity of ellipse is dependent on where was placed point F2, which is convenient as it can be tweaked for better orbit parameters.
///
/// Each step of this solution is pretty strait forward. With help of hyperbola, given data of 1 focus and 2 points is converting to 2 focuses and 1 point.
///
/// With known ellipse parameters it is easy to construct orbit data of transition, and additionally it is possible to calculate mean anomaly of departure and arrival.
/// Difference between these mean anomalies multiplied by mean motion gives exact duration of transition.
/// Because time of transition is depending from second focus point F2 (which is parametric), it is possible to set some specific transition time value
/// and then adjust F2 to match target time as close as possible.
/// </remarks>
public static TrajectoryData CalcTransitionTrajectory(Vector3d p0, Vector3d p1, Vector3d f0, HyperbolaData hyperbola, double targetDuration, bool isReverseOrbit, double attrMass, double g, double precision, double semiMajorAxisUpperLimit)
{
TrajectoryData result = new TrajectoryData();
double hyperbolaValue = 0.0;
int lastDeltaSign = 0;
int changedDeltaSignCount = 0;
float delta = 0.8f;
double lastDuration = 0;
int tmp = 0;
Vector3d ellipseSecondFocus = new Vector3d();
// Calculate transition multiple times until optimal transition duration not found.
// Usually steps count is not larger than a hundred, so 1000 iterations limit is for fail checking.
while (true && tmp < 1e3)
{
tmp++;
bool isBranch0 = (p0 - f0).magnitude < (p1 - f0).magnitude;
ellipseSecondFocus = hyperbola.GetSamplePointOnBranch(hyperbolicCoordinate: hyperbolaValue, isMainBranch: isBranch0);
var transitionOrbitEllipse = new EllipseData(focus0: f0, focus1: ellipseSecondFocus, p0: p0);
if (KeplerOrbitUtils.DotProduct(transitionOrbitEllipse.Normal, hyperbola.Normal) <= 0)
{
transitionOrbitEllipse.AxisSecondary *= -1;
}
if (transitionOrbitEllipse.A > semiMajorAxisUpperLimit)
{
break;
}
if (isReverseOrbit)
{
transitionOrbitEllipse.AxisSecondary = -transitionOrbitEllipse.AxisSecondary;
}
result.orbit = new KeplerOrbitData(
eccentricity: transitionOrbitEllipse.Eccentricity,
semiMajorAxis: transitionOrbitEllipse.AxisMain * transitionOrbitEllipse.A,
semiMinorAxis: transitionOrbitEllipse.AxisSecondary * transitionOrbitEllipse.B,
meanAnomalyDeg: 0,
attractorMass: attrMass,
gConst: g);
result.EccAnomStart = transitionOrbitEllipse.GetEccentricAnomalyForPoint(p0);
result.EccAnomEnd = transitionOrbitEllipse.GetEccentricAnomalyForPoint(p1);
if (result.EccAnomStart > result.EccAnomEnd)
{
result.EccAnomEnd += KeplerOrbitUtils.PI_2;
}
var meanAnomStart = KeplerOrbitUtils.ConvertEccentricToMeanAnomaly(result.EccAnomStart, eccentricity: result.orbit.Eccentricity);
var meanAnomEnd = KeplerOrbitUtils.ConvertEccentricToMeanAnomaly(result.EccAnomEnd, eccentricity: result.orbit.Eccentricity);
var meanAnomDiff = meanAnomEnd - meanAnomStart;
result.Duration = meanAnomEnd <= meanAnomStart ? 0.0 : meanAnomDiff / result.orbit.MeanMotion;
var diff = result.Duration - targetDuration;
int sign = diff >= 0 ? -1 : 1;
if (KeplerOrbitUtils.Abs(diff) < precision)
{
break;
}
if (sign != lastDeltaSign)
{
lastDeltaSign = sign;
changedDeltaSignCount++;
}
if (changedDeltaSignCount >= 2)
{
delta *= 0.5f;
}
int conicShapeAligmentSign = KeplerOrbitUtils.DotProduct(transitionOrbitEllipse.Normal, hyperbola.Normal) >= 0 ? 1 : -1;
hyperbolaValue += delta * sign * conicShapeAligmentSign;
var stepDurationDiff = result.Duration - lastDuration;
if (KeplerOrbitUtils.Abs(stepDurationDiff) < precision)
{
break;
}
lastDuration = result.Duration;
}
return(result);
}
