| public static ProjectOnPlane ( Vector3D, vector, Vector3D, planeNormal ) : Vector3D, | ||
| vector | Vector3D, | |
| planeNormal | Vector3D, | |
| 리턴 | Vector3D, | |
public override bool Update(BasePilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Vector3D forward = wm.GetDirectionVector(ReferenceForward);
Vector3D up = wm.GetDirectionVector(ReferenceUp);
Vector3D right = forward.Cross(up);
double max_up_accel = owner.GetMaxAccelerationFor(up);
Vector3D currentV = linearV;
Goal.Update(owner.elapsedTime);
//determine radial velocity component
Vector3D my_projection = wm.Translation;
ProjectOntoSphere(ref my_projection);
owner.Log?.Invoke($"Me: {my_projection}\n");
Vector3D radius = my_projection - Sphere.Center;
radius.Normalize();
Vector3D radial_direction = my_projection - wm.Translation;
double radial_distance = radial_direction.Normalize();
owner.Log?.Invoke($"Radial error: {radial_distance:F1}\n");
//determine vector pointing towards the projection of the target
Vector3D goal_projection = Goal.Position;
ProjectOntoSphere(ref goal_projection);
owner.Log?.Invoke($"Goal: {goal_projection}\n");
//determine tangential velocity component
Vector3D goal_direction = goal_projection - wm.Translation;
double goal_distance = goal_direction.Normalize();
owner.Log?.Invoke($"Distance: {goal_distance:F1}\n");
Vector3D tangent_direction = Vector3D.ProjectOnPlane(ref goal_direction, ref radius);
if (tangent_direction.Normalize() < OrientationEpsilon)
{ //if our goal is directly opposite of us, just go in the direction we are facing
tangent_direction = wm.GetDirectionVector(ReferenceForward);
tangent_direction = Vector3D.ProjectOnPlane(ref tangent_direction, ref radius);
if (tangent_direction.Normalize() < OrientationEpsilon)
{ //if we are facing straight down or up, use "down" direction instead
tangent_direction = -wm.GetDirectionVector(ReferenceUp);
tangent_direction = Vector3D.ProjectOnPlane(ref tangent_direction, ref radius);
tangent_direction.Normalize();
}
}
double goal_speed = MaxSpeedFor(owner.GetMaxAccelerationFor(-tangent_direction), goal_distance);
linearV = tangent_direction * goal_speed;
linearV += radial_direction * MaxSpeedFor(owner.GetMaxAccelerationFor(-radial_direction), radial_distance);
double diff = owner.RotationAid.Rotate(owner.elapsedTime, tangent_direction, radius, forward, up, ref angularV);
angularV.X += tangent_direction.Dot(forward) * currentV.Dot(tangent_direction) / (Sphere.Radius + radial_distance);
angularV.Z += tangent_direction.Dot(right) * currentV.Dot(tangent_direction) / (Sphere.Radius + radial_distance);
if (goal_distance < PositionEpsilon || diff < OrientationEpsilon)
{
angularV = Vector3D.Zero;
}
return(goal_distance < PositionEpsilon);
}
/// <summary>
/// Gets the Closest point at the center of the rings torus to the given position.
/// </summary>
/// <param name="position">The position to get the closest point to</param>
/// <returns>The closest position to position inside the ring</returns>
public Vector3D ClosestPointAtRingCenter(Vector3D position)
{
if (Contains(position) == ContainmentType.Contains)
{
return(position);
}
Vector3D relPosition = Vector3D.Subtract(position, center);
Vector3D planeNormal = normal;
Vector3D horVector = Vector3D.ProjectOnPlane(ref relPosition, ref planeNormal);
double length = horVector.Length();
if (length < radius + width && length > radius)
{
return(Vector3D.Add(horVector, center));
}
Vector3D horVectorNorm = Vector3D.Normalize(horVector);
return(Vector3D.Add(Vector3D.Multiply(horVectorNorm, radius + width / 2), center));
}
/**
* Given a point with local and move it from depending on O, A et rotation Axis parameter
* point = Point in local coordinate
* ex 1. point = {0, 0, 0} => output = O
* ex 2. point = {oaLength, 0, 0} => output = A
* ex 3. point = {0,oaLength,0} => output = looking from rotationAxis, angle OA OPoint = Pi/2 (90°)
* O = center of rotation (point in world coordinate)
* A = Point for min/max latitude on rotationAxis (point in world coordinate)
* rotationAxis = vector for rotation axis
*/
private List <Vector3D> getListWaypointMinimalOrbit(Vector3D O, Vector3D A, Vector3D rotationAxis, List <Vector3D> points)
{
Vector3D vOA = A - O;
// Rot 1 - Rotate on Z to Align X axis with OA vector
Vector3D vAonXY = new Vector3D(A.X - O.X, A.Y - O.Y, 0);
double signeAngle1 = Math.Sign(Vector3D.Dot(Vector3D.UnitY, vAonXY));
double angle1 = signeAngle1 * GetAngle(Vector3D.UnitX, vAonXY);
Vector3D axeRot1 = Vector3D.UnitZ;
Vector3D x1 = Rotate(Vector3D.UnitX, angle1, axeRot1);
Vector3D y1 = Rotate(Vector3D.UnitY, angle1, axeRot1);
Vector3D z1 = Rotate(Vector3D.UnitZ, angle1, axeRot1);
// Rot 2 - Rotate on Y1 to match X1 with OA
double angle2 = GetAngle(Vector3D.UnitZ, vOA) - Math.PI / 2;
Vector3D axeRot2 = y1;
/// x2 == vOA
Vector3D x2 = Rotate(x1, angle2, axeRot2);
Vector3D y2 = Rotate(y1, angle2, axeRot2);
Vector3D z2 = Rotate(z1, angle2, axeRot2);
// Rot 3 - Rotate depending on rotation Axis given (wanted)
Vector3D vRotFinalOnEquator = Vector3D.ProjectOnPlane(ref rotationAxis, ref vOA);
double signeAngle3 = Math.Sign(Vector3D.Dot(-y2, vRotFinalOnEquator));
double angle3 = signeAngle3 * GetAngle(z2, vRotFinalOnEquator);
Vector3D axeRot3 = x2;
List <Vector3D> output = new List <Vector3D>();
foreach (Vector3D point in points)
{
Vector3D rotatedPoint = point;
rotatedPoint = Rotate(rotatedPoint, angle1, axeRot1);
rotatedPoint = Rotate(rotatedPoint, angle2, axeRot2);
rotatedPoint = Rotate(rotatedPoint, angle3, axeRot3);
rotatedPoint = Translate(rotatedPoint, O);
output.Add(rotatedPoint);
}
return(output);
}
/// <summary>
/// Checks whether a given point is inside the asteroid rings shape or outside.
/// </summary>
/// <param name="point">The point to check</param>
/// <returns>An ContainmentType enum</returns>
public ContainmentType Contains(Vector3D point)
{
Vector3D relPosition = Vector3D.Subtract(point, center);
Vector3D planeNormal = normal;
Vector3D horVector = Vector3D.ProjectOnPlane(ref relPosition, ref planeNormal);
Vector3D vertVector = Vector3D.Subtract(relPosition, horVector);
double length = horVector.Length();
if (length > radius + width || length < radius)
{
return(ContainmentType.Disjoint);
}
double vertLength = vertVector.Length();
double ringHeight = GetHeightAtRad(length);
if (vertLength <= ringHeight)
{
return(ContainmentType.Contains);
}
return(ContainmentType.Disjoint);
}
