public double[] ComputeWeight(Point3d p1, Point3d p2, Point3d p3, Vector3d v)
{
Vector3d N = new Plane(p1, p2, p3).Normal;
if (N.IsParallelTo(v) == -1)
{
double[] ds = new Double[3]; return ds;
}
Vector3d y2 = Vector3d.CrossProduct(v, N);
v = Vector3d.CrossProduct(y2, N);
double t = p1.DistanceTo(p2) + p2.DistanceTo(p3) + p3.DistanceTo(p1);
v.Unitize(); v *= t;
Point3d cen = (p1 + p2 + p3) / 3;
Line l1 = new Line(cen - v, cen + v);
Point3d P1 = l1.ClosestPoint(p1, true);
Point3d P2 = l1.ClosestPoint(p2, true);
Point3d P3 = l1.ClosestPoint(p3, true);
double t1 = (P1.DistanceTo(l1.From)) / l1.Length;
double t2 = (P2.DistanceTo(l1.From)) / l1.Length;
double t3 = (P3.DistanceTo(l1.From)) / l1.Length;
if ((t1 < t2) && (t2 < t3))
{
double[] ds = { 0, (t2 - t1) / (t3 - t1), 1 }; return ds;
}
else if (t1 < t3 && t3 < t2)
{
double[] ds = { 0, 1, (t3 - t1) / (t2 - t1) }; return ds;
}
////
else if (t2 < t1 && t1 < t3)
{
double[] ds = { (t1 - t2) / (t3 - t2), 0, 1 }; return ds;
}
else if (t2 < t3 && t3 < t1)
{
double[] ds = { 1, 0, (t3 - t2) / (t1 - t2) }; return ds;
}
////
else if (t3 < t1 && t1 < t2)
{
double[] ds = { (t1 - t3) / (t2 - t3), 1, 0 }; return ds;
}
else if (t3 < t2 && t2 < t1)
{
double[] ds = { 1, (t2 - t3) / (t1 - t3), 0 }; return ds;
}
else
{
double[] ds = new Double[3]; return ds;
}
}
public static void Hull(List<Point3d> Pts, ref Mesh Msh)
{
List<int> Indices = new List<int>();
Line HullStart = new Line(Pts[0], Pts[1]);
Indices.Add(0); Indices.Add(1);
bool PlaneSet = false;
Plane PlaneStart = Plane.Unset;
for (int P = 2; P < Pts.Count; P++)
{
if (!PlaneSet)
{
if (HullStart.ClosestPoint(Pts[P],false).DistanceTo(Pts[P]) > RhinoDoc.ActiveDoc.ModelAbsoluteTolerance)
{
PlaneSet = true;
Indices.Add(P);
PlaneStart = new Plane(Pts[0], Pts[1], Pts[P]);
}
}
else
{
if (PlaneStart.DistanceTo(Pts[P]) > RhinoDoc.ActiveDoc.ModelAbsoluteTolerance)
{
Indices.Add(P);
break;
}
}
}
Indices.Reverse();
foreach (int I in Indices)
{
Msh.Vertices.Add(Pts[I]);
Pts.RemoveAt(I);
}
Msh.Faces.AddFace(new MeshFace(0, 1, 2));
Msh.Faces.AddFace(new MeshFace(0, 2, 3));
Msh.Faces.AddFace(new MeshFace(0, 3, 1));
Msh.Faces.AddFace(new MeshFace(1, 2, 3));
do
{
NormaliseMesh(ref Msh);
GrowHull(ref Msh, Pts[0]);
Pts.RemoveAt(0);
} while (Pts.Count > 0);
Msh.Vertices.CullUnused();
}
private void intersect2BoundaryPlanes(Vertex vertex)
{
Line intersectionLine = new Line();
Boolean intersectionWork = Rhino.Geometry.Intersect.Intersection.PlanePlane(proxies[vertex.connectedFaces[0].index].rhinoPlane, proxies[vertex.connectedFaces[1].index].rhinoPlane, out intersectionLine);
if (intersectionWork)
{
intersectionLine.Extend(1000, 1000);
Point3d newPositionForVertex = intersectionLine.ClosestPoint(UsefulFunctions.convertVertexToPoint3d(vertex), false);
vertex.position = UsefulFunctions.convertPoint3dToVector(newPositionForVertex);
}
}
/// <summary>
/// Subpart of Mover() that runs when ship is inside destination radius.
/// </summary>
private void Move_Land()
{
if (IsLocked())
{
m_logger.debugLog("Attached!", "Move_Land()", Logger.severity.INFO);
m_navSet.OnTaskComplete_NavRot();
m_mover.StopMove(false);
m_mover.StopRotate();
return;
}
switch (m_landingState)
{
case LandingState.None:
{
if (m_navSet.Settings_Current.Stay_In_Formation)
{
m_logger.debugLog("Maintaining relative position to target", "Move_Land()");
m_mover.CalcMove(m_navBlock, m_navBlock.WorldPosition, m_gridFinder.Grid.GetLinearVelocity());
}
else
{
m_logger.debugLog("Arrived at target", "Move_Land()", Logger.severity.INFO);
m_navSet.OnTaskComplete_NavMove();
m_mover.StopMove();
m_mover.StopRotate();
}
return;
}
case LandingState.Approach:
{
m_navSet.Settings_Task_NavRot.NavigatorRotator = this;
m_navSet.Settings_Task_NavRot.NavigationBlock = m_navBlock;
if (m_landGearWithoutTargetBlock)
{
m_landingState = LandingState.Catch;
goto case LandingState.Catch;
}
else
{
m_landingState = LandingState.Holding;
goto case LandingState.Holding;
}
}
case LandingState.Holding:
{
if (m_gridFinder.Block != null)
{
m_logger.debugLog("Have a block, starting landing sequence", "Move_Land()", Logger.severity.DEBUG);
m_landingState = LandingState.LineUp;
return;
}
Vector3 destination = m_targetPosition;
Vector3 directAway = m_navBlockPos - destination;
if (directAway.LengthSquared() < 1)
{
destination = m_gridFinder.Grid.Entity.WorldAABB.Center;
directAway = m_navBlockPos - destination;
}
Vector3D targetPosition = destination + Vector3.Normalize(directAway) * m_navSet.Settings_Current.DestinationRadius;
m_logger.debugLog("destination: " + destination + ", directAway: " + Vector3.Normalize(directAway) + ", DestinationRadius: " + m_navSet.Settings_Current.DestinationRadius + ", targetPosition: " + targetPosition, "Move_Land()");
m_mover.CalcMove(m_navBlock, targetPosition, m_gridFinder.Grid.GetLinearVelocity());
return;
}
case LandingState.LineUp:
{
if (m_gridFinder.Block == null)
{
m_logger.debugLog("lost block", "Move_Land()");
m_landingState = LandingState.Holding;
return;
}
if (m_navSet.Settings_Current.Distance < 10f)
{
m_logger.debugLog("Reached line: " + m_navSet.Settings_Current.Distance, "Move_Land()");
m_landingState = LandingState.Landing;
return;
}
// move to line from target block outwards
Vector3D landFaceVector = GetLandingFaceVector();
Line destinationLine = new Line(m_targetPosition + landFaceVector * 20, m_targetPosition + landFaceVector * 1000);
Vector3D closestPoint = destinationLine.ClosestPoint(m_navBlockPos);
//m_logger.debugLog("Flying to closest point on line between " + destinationLine.From + " and " + destinationLine.To + " which is " + closestPoint, "Move_Land()");
m_mover.CalcMove(m_navBlock, closestPoint, m_gridFinder.Grid.GetLinearVelocity());
return;
}
case LandingState.Landing:
{
if (m_gridFinder.Block == null)
{
m_logger.debugLog("lost block", "Move_Land()");
m_landingState = LandingState.Holding;
return;
}
if (m_navSet.Settings_Current.DistanceAngle > 0.1f)
{
m_logger.debugLog("waiting for direction to match", "Move_Land()");
m_mover.CalcMove(m_navBlock, m_navBlockPos, m_gridFinder.Grid.GetLinearVelocity(), true);
return;
}
// the autopilot was sometimes freaking out and running away when it got really close
if (m_navSet.DistanceLessThan(1f))
m_navSet.Settings_Task_NavMove.DestinationEntity = m_gridFinder.Grid.Entity;
LockConnector();
float distanceBetween = m_gridFinder.Block.GetLengthInDirection(m_landingDirection) * 0.5f + m_landingHalfSize;
m_logger.debugLog("moving to " + (m_targetPosition + GetLandingFaceVector() * distanceBetween) + ", distance: " + m_navSet.Settings_Current.Distance, "Move_Land()");
m_mover.CalcMove(m_navBlock, m_targetPosition + GetLandingFaceVector() * distanceBetween, m_gridFinder.Grid.GetLinearVelocity(), true);
return;
}
case LandingState.Catch:
{
if (m_navSet.Settings_Current.DistanceAngle > 0.1f)
{
m_logger.debugLog("waiting for direction to match", "Move_Land()");
m_mover.CalcMove(m_navBlock, m_navBlockPos, m_gridFinder.Grid.GetLinearVelocity(), true);
return;
}
m_logger.debugLog("moving to " + m_targetPosition, "Move_Land()");
m_mover.CalcMove(m_navBlock, m_targetPosition, m_gridFinder.Grid.GetLinearVelocity(), true);
return;
}
}
}
public override bool Collide(Line line, float dx = 0, float dy = 0)
{
Vector2 position = AbsolutePosition;
position.X += dx;
position.Y += dy;
Vector2 closest = line.ClosestPoint(position);
return position.LengthSquared(closest) < (Radius * Radius);
}
/// <summary>
/// Subpart of Mover() that runs when ship is inside destination radius.
/// </summary>
private void Move_Land()
{
if (IsLocked())
{
m_logger.debugLog("Attached!", Logger.severity.INFO);
m_navSet.OnTaskComplete(m_settingLevel);
UnreserveTarget();
m_mover.StopMove(false);
m_mover.StopRotate();
if (m_navSet.Shopper != null)
{
m_logger.debugLog("starting shopper");
m_navSet.Shopper.Start();
}
return;
}
switch (m_landingState)
{
case LandingState.None:
{
if (m_navSet.Settings_Current.Stay_In_Formation)
{
m_logger.debugLog("Maintaining relative position to target");
m_mover.CalcMove(m_navBlock, m_navBlock.WorldPosition, m_gridFinder.Grid.GetLinearVelocity());
}
else
{
if (m_navSet.DirectionMatched())
{
m_logger.debugLog("Arrived at target", Logger.severity.INFO);
m_navSet.OnTaskComplete(m_settingLevel);
UnreserveTarget();
m_mover.StopRotate();
}
m_mover.StopMove();
}
return;
}
case LandingState.Approach:
{
m_navSet.GetSettingsLevel(m_settingLevel).NavigatorRotator = this;
m_navSet.GetSettingsLevel(m_settingLevel).NavigationBlock = m_navBlock;
if (m_landGearWithoutTargetBlock)
{
m_landingState = LandingState.Catch;
goto case LandingState.Catch;
}
else
{
m_landingState = LandingState.Holding;
goto case LandingState.Holding;
}
}
case LandingState.Holding:
{
if (m_gridFinder.Block != null)
{
m_logger.debugLog("Have a block, starting landing sequence", Logger.severity.DEBUG);
m_landingState = LandingState.LineUp;
return;
}
m_mover.CalcMove(m_navBlock, m_navBlock.WorldPosition, m_gridFinder.Grid.GetLinearVelocity());
return;
}
case LandingState.LineUp:
{
if (m_gridFinder.Block == null)
{
m_logger.debugLog("lost block");
m_landingState = LandingState.Holding;
return;
}
if (m_navSet.DirectionMatched())
{
if (m_navSet.Settings_Current.Distance < 1f)
{
m_logger.debugLog("Reached line: " + m_navSet.Settings_Current.Distance);
m_landingState = LandingState.Landing;
return;
}
}
else if (!m_mover.Pathfinder.CanRotate)
{
Vector3 destination = m_targetPosition;
Vector3 directAway = m_navBlock.WorldPosition - destination;
if (directAway.LengthSquared() < 1)
{
destination = m_gridFinder.Grid.Entity.WorldAABB.Center;
directAway = m_navBlock.WorldPosition - destination;
}
Vector3D targetPosition = destination + Vector3.Normalize(directAway) * m_navSet.Settings_Current.DestinationRadius;
m_logger.debugLog("Pathfinder cannot rotate, moving away. destination: " + destination + ", directAway: " + Vector3.Normalize(directAway) +
", DestinationRadius: " + m_navSet.Settings_Current.DestinationRadius + ", targetPosition: " + targetPosition);
m_mover.CalcMove(m_navBlock, targetPosition, m_gridFinder.Grid.GetLinearVelocity());
return;
}
else if (m_navSet.Settings_Current.Distance < 1f)
{
// replace do nothing (line) or other rotator that is preventing ship from landing
m_navSet.Settings_Task_NavWay.NavigatorRotator = this;
}
// move to line from target block outwards
Vector3D landFaceVector = GetLandingFaceVector();
float distanceBetween = m_gridFinder.Block.GetLengthInDirection(m_landingDirection) * 0.5f + m_landingHalfSize + 1f;
Line destinationLine = new Line(m_targetPosition + landFaceVector * distanceBetween, m_targetPosition + landFaceVector * 1000f);
Vector3D closestPoint = destinationLine.ClosestPoint(m_navBlock.WorldPosition);
//m_logger.debugLog("Flying to closest point on line between " + destinationLine.From + " and " + destinationLine.To + " which is " + closestPoint, "Move_Land()");
m_mover.CalcMove(m_navBlock, closestPoint, m_gridFinder.Grid.GetLinearVelocity());
return;
}
case LandingState.Landing:
{
if (m_gridFinder.Block == null)
{
m_logger.debugLog("lost block");
m_landingState = LandingState.Holding;
return;
}
if (m_navSet.Settings_Current.DistanceAngle > 0.1f)
{
m_logger.debugLog("waiting for direction to match");
m_mover.CalcMove(m_navBlock, m_navBlock.WorldPosition, m_gridFinder.Grid.GetLinearVelocity());
return;
}
LockConnector();
float distanceBetween = m_gridFinder.Block.GetLengthInDirection(m_landingDirection) * 0.5f + m_landingHalfSize + 0.1f;
//m_logger.debugLog("moving to " + (m_targetPosition + GetLandingFaceVector() * distanceBetween) + ", distance: " + m_navSet.Settings_Current.Distance, "Move_Land()");
if (m_navSet.DistanceLessThan(1f))
{
m_landingSpeedFudge += 0.0001f;
if (m_landingSpeedFudge > 0.1f)
m_landingSpeedFudge = -0.1f;
}
Vector3 fudgeDistance = m_gridFinder.Grid.GetLinearVelocity() * m_landingSpeedFudge;
m_mover.CalcMove(m_navBlock, m_targetPosition + GetLandingFaceVector() * distanceBetween + fudgeDistance, m_gridFinder.Grid.GetLinearVelocity(), m_landingFriend);
return;
}
case LandingState.Catch:
{
if (m_navSet.Settings_Current.DistanceAngle > 0.1f)
{
m_logger.debugLog("waiting for direction to match");
m_mover.CalcMove(m_navBlock, m_navBlock.WorldPosition, m_gridFinder.Grid.GetLinearVelocity());
return;
}
m_logger.debugLog("moving to " + m_targetPosition);
m_mover.CalcMove(m_navBlock, m_targetPosition, m_gridFinder.Grid.GetLinearVelocity(), m_landingFriend);
return;
}
}
}