protected override void Layout()
{
// Determine whether the node is in front of the camera and pointed toward it
MatrixD camMatrix = MyAPIGateway.Session.Camera.WorldMatrix;
Vector3D camOrigin = camMatrix.Translation,
camForward = camMatrix.Forward,
nodeOrigin = PlaneToWorldRef[0].Translation,
nodeForward = PlaneToWorldRef[0].Forward;
IsInFront = Vector3D.Dot((nodeOrigin - camOrigin), camForward) > 0;
IsFacingCamera = IsInFront && Vector3D.Dot(nodeForward, camForward) > 0;
MatrixD worldToPlane;
MatrixD.Invert(ref PlaneToWorldRef[0], out worldToPlane);
LineD cursorLine = HudMain.Cursor.WorldLine;
PlaneD plane = new PlaneD(PlaneToWorldRef[0].Translation, PlaneToWorldRef[0].Forward);
Vector3D worldPos = plane.Intersection(ref cursorLine.From, ref cursorLine.Direction);
Vector3D planePos;
Vector3D.TransformNoProjection(ref worldPos, ref worldToPlane, out planePos);
CursorPos = new Vector3()
{
X = (float)planePos.X,
Y = (float)planePos.Y,
Z = (float)Math.Round(Vector3D.DistanceSquared(worldPos, cursorLine.From), 6)
};
}
// Drag operation performed per update
private void PerformDragg(bool lockToAxis = true)
{
var ray = CreateRayFromCursorPosition();
// Calculate the intersection point
var planeIntersectionPoint = m_dragPlane.Intersection(ref ray.From, ref ray.Direction);
// Calculate the offset
var dragDelta = planeIntersectionPoint - m_dragStartingPoint;
if (lockToAxis)
{
// axis must be unit vector in order to project right
var deltaProjection = dragDelta.Dot(ref m_dragAxis);
// Do nothing for small deltas
if (Math.Abs(deltaProjection) < double.Epsilon)
{
return;
}
// Create new world matrix
var worldMat = ControlledEntity.PositionComp.WorldMatrix;
worldMat.Translation = m_dragStartingPosition + m_dragAxis * deltaProjection;
SetWorldMatrix(ref worldMat);
}
else
{
// Prevent small changes
if (dragDelta.LengthSquared() < double.Epsilon)
{
return;
}
// Create new world matrix
var worldMat = ControlledEntity.PositionComp.WorldMatrix;
worldMat.Translation = m_dragStartingPosition + dragDelta;
SetWorldMatrix(ref worldMat);
}
if (ControlledEntity.Physics != null)
{
ControlledEntity.Physics.ClearSpeed();
}
UpdateGizmoPosition();
}
public override bool Update(AutoPilot owner, ref Vector3D linearV, ref Vector3D angularV)
{
if (Goal == null)
{
return(false);
}
IMyCubeBlock reference = Reference ?? owner.Controller;
MatrixD wm = reference.WorldMatrix;
Goal.UpdateTime(owner.elapsedTime);
Vector3D currentGoalPos = Goal.CurrentPosition;
Vector3D direction = currentGoalPos - wm.Translation;
double distance = direction.Normalize();
double target_distance = distance;
double diff;
if (!Vector3D.IsZero(Approach))
{
Vector3D minusApproach = -Approach;
diff = owner.RotateToMatch(Approach, Facing,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
PlaneD alignment = new PlaneD(wm.Translation, minusApproach);
Vector3D alignedPos = alignment.Intersection(ref currentGoalPos, ref minusApproach);
Vector3D correction = alignedPos - wm.Translation;
if (!Vector3D.IsZero(correction, PositionEpsilon)) //are we on approach vector?
{ //no - let's move there
direction = correction;
distance = direction.Normalize();
}
//otherwise, we can keep our current direction
}
else
{
diff = owner.RotateToMatch(direction, Facing,
wm.GetDirectionVector(ReferenceForward),
wm.GetDirectionVector(ReferenceUp),
ref angularV);
}
//rotate the ship to face it
if (diff > OrientationEpsilon) //we still need to rotate
{
linearV = Goal.Velocity; //match velocities with our target, then.
}
else //we are good
{
//how quickly can we go, assuming we still need to stop at the end?
double accel = owner.GetMaxAccelerationFor(-direction);
double braking_time = Math.Sqrt(2 * distance / accel);
double acceptable_velocity = Math.Min(VelocityUsage * accel * braking_time, MaxLinearSpeed);
//extra slowdown when close to the target
acceptable_velocity = Math.Min(acceptable_velocity, distance);
//moving relative to the target
linearV = direction * acceptable_velocity + Goal.Velocity;
angularV = Vector3D.Zero;
}
return(TryLockIn(distance) || (target_distance < PositionEpsilon));
}
// Control was picked, prepare for rotation operation
private void PrepareRotation(Vector3D axis)
{
m_rotationAxis = axis;
// Create plane for picking of rotation vectors
m_rotationPlane = new PlaneD(m_gizmoMatrix.Translation, m_rotationAxis);
// Set up the starting point of rotation
m_rotationStartingPoint = m_rotationPlane.Intersection(ref m_lastRay.From, ref m_lastRay.Direction);
// Store the transformations
var worldMat = ControlledEntity.PositionComp.WorldMatrix;
m_storedScale = MatrixD.CreateScale(worldMat.Scale);
m_storedTranslation = worldMat.Translation;
m_storedOrientation = worldMat.GetOrientation();
}
// Rotation operation performed per update
private void PerformRotation()
{
var ray = CreateRayFromCursorPosition();
// Calculate the intersection point
var planeIntersectionPoint = m_rotationPlane.Intersection(ref ray.From, ref ray.Direction);
// Prevent too small steps
if (Vector3D.DistanceSquared(m_rotationStartingPoint, planeIntersectionPoint) < double.Epsilon * 20)
{
return;
}
var fromRotationVector = m_rotationStartingPoint - m_gizmoMatrix.Translation;
var toRotationVector = planeIntersectionPoint - m_gizmoMatrix.Translation;
var rotationMat = MatrixD.CreateFromQuaternion(QuaternionD.CreateFromTwoVectors(fromRotationVector, toRotationVector));
var resultMat = m_storedOrientation * rotationMat * m_storedScale;
resultMat.Translation = m_storedTranslation;
SetWorldMatrix(ref resultMat);
}
// Control was picked, prepare for rotation operation
private void PrepareRotation(Vector3D axis)
{
m_rotationAxis = axis;
// Create plane for picking of rotation vectors
m_rotationPlane = new PlaneD(m_gizmoMatrix.Translation, m_rotationAxis);
// Set up the starting point of rotation
m_rotationStartingPoint = m_rotationPlane.Intersection(ref m_lastRay.From, ref m_lastRay.Direction);
// Store the transformations
var worldMat = ControlledEntity.PositionComp.WorldMatrix;
m_storedScale = MatrixD.CreateScale(worldMat.Scale);
m_storedTranslation = worldMat.Translation;
m_storedOrientation = worldMat.GetOrientation();
}