| public static Dot ( Vector3D, lhs, Vector3D, rhs ) : double | ||
| lhs | Vector3D, | |
| rhs | Vector3D, | |
| return | double | |
public static bool TryRayPlane(
Vector3D rayOrigin,
Vector3D rayDirection,
Vector3D planeNormal,
double planeD,
out Vector3D intersectionPoint)
{
double denominator = planeNormal.Dot(rayDirection);
if (Math.Abs(denominator) < 0.00000000000000000001)
{
//
// Ray is parallel to plane. The ray may be in the polygon's plane.
//
intersectionPoint = Vector3D.Zero;
return false;
}
double t = (-planeD - planeNormal.Dot(rayOrigin)) / denominator;
if (t < 0)
{
intersectionPoint = Vector3D.Zero;
return false;
}
intersectionPoint = rayOrigin + (t * rayDirection);
return true;
}
/// <summary>
/// Calculates the angle subtended by two line segments that meet at a vertex.
/// </summary>
/// <param name="start">The end of one of the line segments.</param>
/// <param name="vertex">The vertex of the angle formed by the two line segments.</param>
/// <param name="end">The end of the other line segment.</param>
/// <returns>The angle subtended by the two line segments in degrees.</returns>
public static double SubtendedAngle(PointF start, PointF vertex, PointF end)
{
Vector3D vertexPositionVector = new Vector3D(vertex.X, vertex.Y, 0);
Vector3D a = new Vector3D(start.X, start.Y, 0) - vertexPositionVector;
Vector3D b = new Vector3D(end.X, end.Y, 0) - vertexPositionVector;
float dotProduct = a.Dot(b);
Vector3D crossProduct = a.Cross(b);
float magA = a.Magnitude;
float magB = b.Magnitude;
if (FloatComparer.AreEqual(magA, 0F) || FloatComparer.AreEqual(magB, 0F))
return 0;
double cosTheta = dotProduct/magA/magB;
// Make sure cosTheta is within bounds so we don't
// get any errors when we take the acos.
if (cosTheta > 1.0f)
cosTheta = 1.0f;
if (cosTheta < -1.0f)
cosTheta = -1.0f;
double theta = Math.Acos(cosTheta)*(crossProduct.Z == 0 ? 1 : -Math.Sign(crossProduct.Z));
double thetaInDegrees = theta/Math.PI*180;
return thetaInDegrees;
}
public static Vector3D GnomonicToStereo( Vector3D g )
{
g /= m_gScale;
double dot = g.Dot( g ); // X^2 + Y^2
double z = -1 / Math.Sqrt( dot + 1 );
return g*z / (z - 1);
}
public static Vector3D KleinToPoincare( Vector3D k )
{
double dot = k.Dot( k );
if( dot > 1 ) // This avoids some NaN problems I saw.
dot = 1;
double mag = (1 - Math.Sqrt( 1 - dot )) / dot;
return k * mag;
}
public static Vector3D PlaneToSphere( Vector3D planePoint )
{
planePoint.Z = 0;
double dot = planePoint.Dot( planePoint ); // X^2 + Y^2
return new Vector3D(
2 * planePoint.X / ( dot + 1 ),
2 * planePoint.Y / ( dot + 1 ),
( dot - 1 ) / ( dot + 1 ) );
}
public static Matrix3D Orthogonalize(Vector3D u, Vector3D v)
{
Vector3D a = u.Normalize();
Vector3D temp = v - (v.Dot(a)) * a;
Vector3D b = temp.Normalize();
Vector3D c = a.Cross(b).Normalize();
Matrix3D e = new Matrix3D(a, b, c);
return e;
}
public static Vector3D R3toS3( Vector3D p )
{
if( Infinity.IsInfinite( p ) )
return new Vector3D( 0, 0, 0, 1 );
p.W = 0;
double dot = p.Dot( p ); // X^2 + Y^2 + Z^2
return new Vector3D(
2 * p.X / ( dot + 1 ),
2 * p.Y / ( dot + 1 ),
2 * p.Z / ( dot + 1 ),
( dot - 1 ) / ( dot + 1 ) );
}
public double DistanceToPoint(Vector3D point)
{
return point.Dot(Normal) + D;
}
Vector3D VectorProjection(Vector3D a, Vector3D b) //project a onto b
{
Vector3D projection = a.Dot(b) / b.LengthSquared() * b;
return(projection);
}
public double Dot(Quaternion q)
{
return(_v.Dot(q._v) + _w * q._w);
}
private static Vector3D Project(Vector3D one, Vector3D two) //project a on b
{
Vector3D projection = one.Dot(two) / two.LengthSquared() * two;
return(projection);
}
static public List <double> RayMeshIntersections(Point3D orig, Vector3D dir, MeshGeometry3D mesh, bool frontFacesOnly)
{
List <double> result = new List <double>();
if (mesh == null || !dir.IsValid())
{
return(result);
}
double epsilon = 1E-9;
Int32Collection indices = mesh.TriangleIndices;
Point3DCollection positions = mesh.Positions;
for (int i = 0; i < indices.Count; i += 3)
{
Point3D vert0 = positions[indices[i]];
Point3D vert1 = positions[indices[i + 1]];
Point3D vert2 = positions[indices[i + 2]];
//--- find vectors for two edges sharing vert0
Vector3D edge1 = vert1 - vert0;
Vector3D edge2 = vert2 - vert0;
//--- begin calculating determinant also used to calculate U parameter
Vector3D pvec = dir.Cross(edge2);
//--- if determinant is near zero ray lies in plane of triangle
double det = edge1.Dot(pvec);
if (det < epsilon)
{
if (frontFacesOnly)
{
continue;
}
else if (det > -epsilon)
{
continue;
}
}
double inv_det = 1.0 / det;
//--- calculate distance from vert0 to ray origin
Vector3D tvec = orig - vert0;
//--- calculate U parameter and test bounds
double u = tvec.Dot(pvec) * inv_det;
if (u < 0.0 || u > 1.0)
{
continue;
}
//--- prepare to test V parameter
Vector3D qvec = tvec.Cross(edge1);
//--- calculate V parameter and test bounds
double v = dir.Dot(qvec) * inv_det;
if (v < 0.0 || u + v > 1.0)
{
continue;
}
//--- calculate t scale parameters, ray intersects triangle
double t = edge2.Dot(qvec) * inv_det;
result.Add(t);
}
return(result);
}
public static Vector3D Reflect(Vector3D inDirection, Vector3D inNormal)
{
return(-2f * Vector3D.Dot(inNormal, inDirection) * inNormal + inDirection);
}
public static Matrix3D ComputeRotationRodrigues(Vector3D first, Vector3D second, int step)
{
Vector3D cross = first.Cross(second);
double sin = cross.Length();
double cos = first.Dot(second);
//if (cos > 0.99)
//{
// return Matrix3D.IdentityMatrix();
//}
double angle = Math.Acos(cos);
cos = Math.Cos(angle / step);
sin = Math.Sin(angle / step);
cross = cross.Normalize();
double x = cross.x;
double y = cross.y;
double z = cross.z;
Matrix3D result = Matrix3D.IdentityMatrix();
//result[0, 0] = cos+(1-cos)*x*x;
//result[0, 1] = x * y*(1 - cos) - z * sin;
//result[0, 2] = y* sin+x*z*(1-cos);
//result[1, 0] = z * sin + x * y * (1 - cos);
//result[1, 1] = cos + y * y * (1 - cos);
//result[1, 2] = -x * sin + y * z * (1 - cos);
//result[2, 0] = -y * sin + x * z * (1 - cos);
//result[2, 1] = x * sin + y * z * (1 - cos);
//result[2, 2] =cos + z* z * (1 - cos);
//result[0, 0] = cos*(y*y+z*z) + x * x;
//result[0, 1] = x * y * (1 - cos) - z * sin;
//result[0, 2] = y * sin + x * z * (1 - cos);
//result[1, 0] = z * sin + x * y * (1 - cos);
//result[1, 1] = cos*(x*x+z*z) + y * y ;
//result[1, 2] = -x * sin + y * z * (1 - cos);
//result[2, 0] = -y * sin + x * z * (1 - cos);
//result[2, 1] = x * sin + y * z * (1 - cos);
//result[2, 2] = cos*(x*x+y*y) + z * z ;
result[0, 0] = cos * (1 - x * x) + x * x;
result[0, 1] = x * y * (1 - cos) - z * sin;
result[0, 2] = y * sin + x * z * (1 - cos);
result[1, 0] = z * sin + x * y * (1 - cos);
result[1, 1] = cos * (1 - y * y) + y * y;
result[1, 2] = -x * sin + y * z * (1 - cos);
result[2, 0] = -y * sin + x * z * (1 - cos);
result[2, 1] = x * sin + y * z * (1 - cos);
result[2, 2] = cos * (1 - z * z) + z * z;
// result = result.Transpose();
return result;
}
private static double planeD(Vector3D origin, Vector3D normal)
{
return -origin.Dot(normal);
}
public static Vector3D Projection(Vector3D a, Vector3D b)
{
Vector3D projection = a.Dot(b) / b.LengthSquared() * b;
return(projection);
}
/// calculate component of one on two
public static double GetProjectionScalar(Vector3D one, Vector3D two)
{
double dotBetween = one.Dot(two);
return(one.Length() * dotBetween);
}
/// project one on two
public static Vector3D Project(Vector3D one, Vector3D two)
{
Vector3D projection = one.Dot(two) / two.LengthSquared() * two;
return(projection);
}
/// returns angle in radians
public static double GetAngle(Vector3D One, Vector3D Two)
{
return(Math.Acos(MathHelper.Clamp(One.Dot(Two) / Math.Sqrt(One.LengthSquared() * Two.LengthSquared()), -1, 1)));
}
public bool Test()
{
Vector3D WdU = Vector3D.Zero;
Vector3D AWdU = Vector3D.Zero;
Vector3D DdU = Vector3D.Zero;
Vector3D ADdU = Vector3D.Zero;
Vector3D AWxDdU = Vector3D.Zero;
double RHS;
Vector3D diff = ray.Origin - box.Center;
WdU[0] = ray.Direction.Dot(box.AxisX);
AWdU[0] = Math.Abs(WdU[0]);
DdU[0] = diff.Dot(box.AxisX);
ADdU[0] = Math.Abs(DdU[0]);
if (ADdU[0] > box.Extent.x && DdU[0] * WdU[0] >= (double)0)
{
return(false);
}
WdU[1] = ray.Direction.Dot(box.AxisY);
AWdU[1] = Math.Abs(WdU[1]);
DdU[1] = diff.Dot(box.AxisY);
ADdU[1] = Math.Abs(DdU[1]);
if (ADdU[1] > box.Extent.y && DdU[1] * WdU[1] >= (double)0)
{
return(false);
}
WdU[2] = ray.Direction.Dot(box.AxisZ);
AWdU[2] = Math.Abs(WdU[2]);
DdU[2] = diff.Dot(box.AxisZ);
ADdU[2] = Math.Abs(DdU[2]);
if (ADdU[2] > box.Extent.z && DdU[2] * WdU[2] >= (double)0)
{
return(false);
}
Vector3D WxD = ray.Direction.Cross(diff);
AWxDdU[0] = Math.Abs(WxD.Dot(box.AxisX));
RHS = box.Extent.y * AWdU[2] + box.Extent.z * AWdU[1];
if (AWxDdU[0] > RHS)
{
return(false);
}
AWxDdU[1] = Math.Abs(WxD.Dot(box.AxisY));
RHS = box.Extent.x * AWdU[2] + box.Extent.z * AWdU[0];
if (AWxDdU[1] > RHS)
{
return(false);
}
AWxDdU[2] = Math.Abs(WxD.Dot(box.AxisZ));
RHS = box.Extent.x * AWdU[1] + box.Extent.y * AWdU[0];
if (AWxDdU[2] > RHS)
{
return(false);
}
return(true);
}
public virtual Color Shade(Vector3D p, Vector3D n, Vector3D v, ArrayList lights,
ArrayList objects, Color bgnd)
{
IEnumerator lightSources = lights.GetEnumerator();
float r = 0;
float g = 0;
float b = 0;
while (lightSources.MoveNext())
{
Light light = (Light)lightSources.Current;
if (light.lightType == Light.AMBIENT)
{
r += ka * ir * light.ir;
g += ka * ig * light.ig;
b += ka * ib * light.ib;
}
else
{
Vector3D l;
if (light.lightType == Light.POINT)
{
l = new Vector3D(light.lvec.x - p.x, light.lvec.y - p.y, light.lvec.z - p.z);
l.Normalize();
}
else
{
l = new Vector3D(-light.lvec.x, -light.lvec.y, -light.lvec.z);
}
// Check if the surface point is in shadow
Vector3D poffset = new Vector3D(p.x + TINY * l.x, p.y + TINY * l.y, p.z + TINY *
l.z);
Ray shadowRay = new Ray(poffset, l);
if (shadowRay.Trace(objects))
{
break;
}
float lambert = Vector3D.Dot(n, l);
if (lambert > 0)
{
if (kd > 0)
{
float diffuse = kd * lambert;
r += diffuse * ir * light.ir;
g += diffuse * ig * light.ig;
b += diffuse * ib * light.ib;
}
if (ks > 0)
{
lambert *= 2;
float spec = v.Dot(lambert * n.x - l.x, lambert * n.y - l.y, lambert * n.z - l.z);
if (spec > 0)
{
spec = ks * ((float)Math.Pow((double)spec, (double)ns));
r += spec * light.ir;
g += spec * light.ig;
b += spec * light.ib;
}
}
}
}
}
// Compute illumination due to reflection
if (kr > 0)
{
float t = v.Dot(n);
if (t > 0)
{
t *= 2;
Vector3D reflect = new Vector3D(t * n.x - v.x, t * n.y - v.y, t * n.z - v.z);
Vector3D poffset = new Vector3D(p.x + TINY * reflect.x, p.y + TINY * reflect.y, p
.z + TINY * reflect.z);
Ray reflectedRay = new Ray(poffset, reflect);
if (reflectedRay.Trace(objects))
{
Color rcolor = reflectedRay.Shade(lights, objects, bgnd);
r += kr * rcolor.GetRed();
g += kr * rcolor.GetGreen();
b += kr * rcolor.GetBlue();
}
else
{
r += kr * bgnd.GetRed();
g += kr * bgnd.GetGreen();
b += kr * bgnd.GetBlue();
}
}
}
// Add code for refraction here
r = (r > 1f) ? 1f : r;
g = (g > 1f) ? 1f : g;
b = (b > 1f) ? 1f : b;
return new Color(r, g, b);
}
private IEnumerable<Vector3D> GetSlicePositions(Vector3D slicerOrigin, Vector3D slicerAxisX, Vector3D slicerAxisY, Vector3D slicerAxisZ, float stackDepth, int sliceRows, int sliceColumns, SizeF pixelSpacing, float sliceSpacing, float sliceThickness, Vector3D startPosition, Vector3D endPosition, int count)
{
// just under 1.5 (+1 is to include the end edge as a slice, and +0.499999 to make it round *down* if the stack depth is almost an exact multiple of the requested spacing)
const double sliceCountRounder = 1 + 0.5 - 1e-6;
// compute the increment in position between consecutive slices
var slicePositionIncrement = slicerAxisZ*sliceSpacing;
// get the number of slices in the output when slicing the entire volume
var sliceCount = (int) (Math.Abs(1.0*stackDepth/sliceSpacing) + sliceCountRounder);
// compute the offset needed to centre the stack along the stacking axis (so that we don't favour either end of the stack if the slice count is rounded)
var firstSliceOffset = (stackDepth - (sliceCount - 1)*sliceSpacing)/2f;
// compute the position of the first slice when slicing the entire volume
var firstSlicePosition = slicerOrigin;
// if a start position (and, optionally, end position or total count) is provided, we adjust the slice position and count accordingly
if (startPosition != null)
{
// NOTE: The overall logic here is that, even if a position is given for the first slice, it does not make sense to arbitrarily
// reslice from that point to the edge of the volume, because the other half of the volume will not be in the output.
// Therefore, we only make use of the individual components of the position for which the client code has provided fixed bounds
// because then the client code is explicitly requested a subrange of the volume in that dimension, and any auto-calculated
// bounds will still include the entire volume in that dimension.
// e.g. we use the X component of the position if the output width is fixed, and the output stack will be horizontally centred on this position
// get the requested start position in the slicer frame
var slicerStart = startPosition - slicerOrigin;
// if the width of the output is fixed, we horizontally centre the slices on the location provided
if (Columns.HasValue || SliceWidth.HasValue)
{
// projecting the start position on to the X axis then subtracting half the width of the image gives us an offset along X to get desired starting location
var offsetX = slicerAxisX.Dot(slicerStart) - sliceColumns*pixelSpacing.Width/2f;
firstSlicePosition += offsetX*slicerAxisX;
}
// if the height of the output is fixed, we vertically centre the slices on the location provided
if (Rows.HasValue || SliceHeight.HasValue)
{
// projecting the start position on to the Y axis then subtracting half the width of the image gives us an offset along Y to get desired starting location
var offsetY = slicerAxisY.Dot(slicerStart) - sliceRows*pixelSpacing.Height/2f;
firstSlicePosition += offsetY*slicerAxisY;
}
// if end position or total slice count is provided, we will slice the selected range only
if (endPosition != null)
{
// projecting the start position on to the Z axis gives us an offset along Z to get desired starting location
var offsetZ = slicerAxisZ.Dot(slicerStart);
firstSlicePosition += offsetZ*slicerAxisZ;
// position is explicitly given, so clear the offset
firstSliceOffset = 0;
// recompute the slice count based on the magnitude of the Z component delta between the end and start positions
var slicerStop = endPosition - slicerOrigin;
var stackRange = slicerAxisZ.Dot(slicerStop) - offsetZ;
sliceCount = (int) (Math.Abs(1.0*stackRange/sliceSpacing) + sliceCountRounder);
}
else if (count > 0)
{
// projecting the start position on to the Z axis gives us an offset along Z to get desired starting location
var offsetZ = slicerAxisZ.Dot(slicerStart);
firstSlicePosition += offsetZ*slicerAxisZ;
// position is explicitly given, so clear the offset
firstSliceOffset = 0;
// the slice count is exactly as provided
sliceCount = count;
}
}
// if necessary, adjust the first slice position in order to centre the stack on the volume
if (!FloatComparer.AreEqual(firstSliceOffset, 0)) firstSlicePosition += firstSliceOffset*slicerAxisZ;
// compute the positions for the requested slices
return Enumerable.Range(0, Math.Max(1, sliceCount)).Select(n => firstSlicePosition + n*slicePositionIncrement);
}
public void Dot()
{
Assert.AreEqual(0.0f, Vector3D.Dot(Vector3D.UnitX, Vector3D.UnitY));
Assert.AreEqual(1.0f, Vector3D.Dot(Vector3D.UnitX, Vector3D.UnitX));
}
private bool GetTextBoxAdjustmentParameters(out PointF startPoint, out PointF endPoint, out float lengthOfLineThroughTextBox)
{
startPoint = _topParent.Points[0];
endPoint = _topParent.Points[1];
lengthOfLineThroughTextBox = 0;
Vector3D lineDirection = new Vector3D(endPoint.X - startPoint.X, endPoint.Y - startPoint.Y, 0F);
if (Vector3D.AreEqual(lineDirection, Vector3D.Null))
return false;
Vector3D lineUnit = lineDirection.Normalize();
Vector3D xUnit = new Vector3D(1F, 0, 0);
Vector3D yUnit = new Vector3D(0, 1F, 0);
float cosThetaX = Math.Abs(xUnit.Dot(lineUnit));
float cosThetaY = Math.Abs(yUnit.Dot(lineUnit));
float textWidth = _textGraphic.BoundingBox.Width;
float textHeight = _textGraphic.BoundingBox.Height;
if (cosThetaX >= cosThetaY)
{
// the distance along the line to where we want the outside right edge of the text to be.
lengthOfLineThroughTextBox = cosThetaX*textWidth;
if (lineDirection.X < 0)
{
startPoint = _topParent.Points[1];
endPoint = _topParent.Points[0];
}
}
else
{
// the distance along the line to where we want the outside bottom edge of the text to be.
lengthOfLineThroughTextBox = cosThetaY*textHeight;
if (lineDirection.Y < 0)
{
startPoint = _topParent.Points[1];
endPoint = _topParent.Points[0];
}
}
return true;
}
public static void DebugDraw()
{
if (MyDebugDrawSettings.ENABLE_DEBUG_DRAW && MyDebugDrawSettings.DEBUG_DRAW_ENTITY_COMPONENTS && MySector.MainCamera != null)
{
double fontSize = 1.5;
double lineSize = fontSize * 0.045;
double hoffset = 0.5f;
Vector3D playerPos = MySector.MainCamera.Position;
Vector3D upVector = MySector.MainCamera.WorldMatrix.Up;
Vector3D rightVector = MySector.MainCamera.WorldMatrix.Right;
Vector3D fwVector = MySector.MainCamera.ForwardVector;
BoundingSphereD bSphere = new BoundingSphereD(playerPos, 5.0f);
var entities = MyEntities.GetEntitiesInSphere(ref bSphere);
foreach (var entity in entities)
{
if (entity.PositionComp == null)
{
continue;
}
Vector3D originalPos = entity.PositionComp.GetPosition();
Vector3D pos2 = originalPos + upVector * 0.1f;
Vector3D pos = pos2 - rightVector * hoffset;
Vector3D viewVector = Vector3D.Normalize(originalPos - playerPos);
double dot = Vector3D.Dot(viewVector, fwVector);
if (dot < 0.9995)
{
MyRenderProxy.DebugDrawSphere(originalPos, 0.01f, Color.White, 1.0f, false);
continue;
}
double dist = Vector3D.Distance(pos, playerPos);
double textSize = Math.Atan(fontSize / Math.Max(dist, 0.001));
float n = 0;
{
var enumerator = entity.Components.GetEnumerator();
MyComponentBase component = null;
while (enumerator.MoveNext())
{
component = enumerator.Current;
n += GetComponentLines(component);
}
n += 1;
n -= GetComponentLines(component); // The last component should not make the line longer
enumerator.Dispose();
}
Vector3D topPos = pos + (n + 0.5f) * upVector * lineSize;
Vector3D currentPos = pos + (n + 1) * upVector * lineSize + 0.01f * rightVector;
MyRenderProxy.DebugDrawLine3D(originalPos, pos2, Color.White, Color.White, false);
MyRenderProxy.DebugDrawLine3D(pos, pos2, Color.White, Color.White, false);
MyRenderProxy.DebugDrawLine3D(pos, topPos, Color.White, Color.White, false);
MyRenderProxy.DebugDrawLine3D(topPos, topPos + rightVector * 1.0f, Color.White, Color.White, false);
MyRenderProxy.DebugDrawText3D(currentPos, entity.GetType().ToString() + " - " + entity.DisplayName, Color.Orange, (float)textSize, false, MyGuiDrawAlignEnum.HORISONTAL_LEFT_AND_VERTICAL_CENTER);
foreach (var component in entity.Components)
{
currentPos = pos + n * upVector * lineSize;
DebugDrawComponent(component, currentPos, rightVector, upVector, lineSize, (float)textSize);
var entityComponent = component as MyEntityComponentBase;
string compType = entityComponent == null ? "" : entityComponent.ComponentTypeDebugString;
MyRenderProxy.DebugDrawText3D(currentPos - 0.02f * rightVector, compType, Color.Yellow, (float)textSize, false, MyGuiDrawAlignEnum.HORISONTAL_RIGHT_AND_VERTICAL_CENTER);
n -= GetComponentLines(component);
}
}
entities.Clear();
}
}
public void TestDot()
{
Vector3D v1 = new Vector3D(2.2F, -6.1F, 7.4F);
Vector3D v2 = new Vector3D(3.8F, 3.7F, 4.1F);
Assert.IsTrue(FloatComparer.AreEqual(v1.Dot(v2), 16.13F));
}
/// <summary>
/// Try to align the ship/grid with the given vector. Returns true if the ship is within minAngleRad of being aligned
/// </summary>
/// <param name="argument">The direction to point. "rocket" (backward), "backward", "up","forward"</param>
/// <param name="vDirection">the vector for the aim.</param>
/// <param name="gyroControlPoint">the terminal block to use for orientation</param>
/// <returns></returns>
bool GyroMain(string argument, Vector3D vDirection, IMyTerminalBlock gyroControlPoint)
{
bool bAligned = true;
if (gyroControl == null)
{
gyrosetup();
}
// Echo("GyroMain(" + argument + ",VECTOR3D) #Gyros=" + gyros.Count);
Matrix or;
gyroControlPoint.Orientation.GetMatrix(out or);
Vector3D down;
argument = argument.ToLower();
if (argument.Contains("rocket"))
{
down = or.Backward;
}
else if (argument.Contains("up"))
{
down = or.Up;
}
else if (argument.Contains("backward"))
{
down = or.Backward;
}
else if (argument.Contains("forward"))
{
down = or.Forward;
}
else
{
down = or.Down;
}
vDirection.Normalize();
for (int i = 0; i < gyros.Count; ++i)
{
var g = gyros[i];
g.Orientation.GetMatrix(out or);
// not really 'down'.. just the direciton we are currently pointing
var localDown = Vector3D.Transform(down, MatrixD.Transpose(or));
// not really gravity. just the direction we want to point
var localGrav = Vector3D.Transform(vDirection, MatrixD.Transpose(g.WorldMatrix.GetOrientation()));
//Since the gyro ui lies, we are not trying to control yaw,pitch,roll but rather we
//need a rotation vector (axis around which to rotate)
var rot = Vector3D.Cross(localDown, localGrav);
double dot2 = Vector3D.Dot(localDown, localGrav);
double ang = rot.Length();
ang = Math.Atan2(ang, Math.Sqrt(Math.Max(0.0, 1.0 - ang * ang)));
if (dot2 < 0)
{
ang = Math.PI - ang; // compensate for >+/-90
}
if (ang < minAngleRad)
{ // close enough
//g.SetValueBool("Override", false);
g.GyroOverride = false;
continue;
}
// Echo("Auto-Level:Off level: "+(ang*180.0/3.14).ToString()+"deg");
float yawMax = g.GetMaximum <float>("Yaw"); // we assume all three are the same max
double ctrl_vel = yawMax * (ang / Math.PI) * CTRL_COEFF;
ctrl_vel = Math.Min(yawMax, ctrl_vel);
ctrl_vel = Math.Max(0.01, ctrl_vel);
rot.Normalize();
rot *= ctrl_vel;
// float pitch = -(float)rot.GetDim(0);
float pitch = -(float)rot.X;
//g.SetValueFloat("Pitch", -pitch);
g.Pitch = pitch;
// float yaw = -(float)rot.GetDim(1);
float yaw = -(float)rot.Y;
//g.SetValueFloat("Yaw", yaw);
g.Yaw = yaw;
// float roll = -(float)rot.GetDim(2);
float roll = -(float)rot.Z;
// g.SetValueFloat("Roll", roll);
g.Roll = roll;
// g.SetValueFloat("Power", 1.0f);
//g.SetValueBool("Override", true);
g.GyroOverride = true;
bAligned = false;
}
return(bAligned);
}
private void ComputeGravityAlignedOrientation(out MatrixD resultOrientationStorage)
{
// Y axis
bool inGravityField = true;
Vector3D upVector = -MyGravityProviderSystem.CalculateTotalGravityInPoint(Position);
if (upVector.LengthSquared() < MyMathConstants.EPSILON)
{
upVector = m_lastUpVec;
m_lastOrientationWeight = 1;
inGravityField = false;
}
else
{
m_lastUpVec = upVector;
}
upVector.Normalize();
// X axis
Vector3D rightVector = m_lastRightVec - Vector3D.Dot(m_lastRightVec, upVector) * upVector;
if (rightVector.LengthSquared() < MyMathConstants.EPSILON)
{
rightVector = m_orientation.Right - Vector3D.Dot(m_orientation.Right, upVector) * upVector;
// backup behavior if singularity happens
if (rightVector.LengthSquared() < MyMathConstants.EPSILON)
{
rightVector = m_orientation.Forward - Vector3D.Dot(m_orientation.Forward, upVector) * upVector;
}
// backup behavior if singularity happens
}
rightVector.Normalize();
m_lastRightVec = rightVector;
// Z axis
Vector3D forwardVector;
Vector3D.Cross(ref upVector, ref rightVector, out forwardVector);
resultOrientationStorage = MatrixD.Identity;
resultOrientationStorage.Right = rightVector;
resultOrientationStorage.Up = upVector;
resultOrientationStorage.Forward = forwardVector;
resultOrientationStorage = MatrixD.CreateFromAxisAngle(Vector3D.Right, m_pitch) * resultOrientationStorage *
MatrixD.CreateFromAxisAngle(upVector, m_yaw);
upVector = resultOrientationStorage.Up;
rightVector = resultOrientationStorage.Right;
resultOrientationStorage.Right = Math.Cos(m_roll) * rightVector + Math.Sin(m_roll) * upVector;
resultOrientationStorage.Up = -Math.Sin(m_roll) * rightVector + Math.Cos(m_roll) * upVector;
if (inGravityField && m_lastOrientationWeight > 0)
{
m_lastOrientationWeight = Math.Max(0, m_lastOrientationWeight - MyEngineConstants.UPDATE_STEP_SIZE_IN_SECONDS);
resultOrientationStorage = MatrixD.Slerp(resultOrientationStorage, m_lastOrientation,
MathHelper.SmoothStepStable(m_lastOrientationWeight));
resultOrientationStorage = MatrixD.Orthogonalize(resultOrientationStorage);
resultOrientationStorage.Forward = Vector3D.Cross(resultOrientationStorage.Up, resultOrientationStorage.Right);
}
if (!inGravityField)
{
m_lastOrientation = resultOrientationStorage;
}
}
void Control()
{
var brakes = Controller.MoveIndicator.Y > 0 || Controller.HandBrake;
var velocity = Vector3D.TransformNormal(Controller.GetShipVelocities().LinearVelocity, MatrixD.Transpose(Controller.WorldMatrix)) * brakingConstant;
avgWheelPosition = Vector3D.Zero;
foreach (var w in Wheels)
{
avgWheelPosition += w.GetPosition();
}
avgWheelPosition /= Wheels.Count;
foreach (var w in SubgridWheels)
{
w.SetValue("Propulsion override", -Math.Sign(Math.Round(Vector3D.Dot(w.WorldMatrix.Up, Controller.WorldMatrix.Right), 2)) * (Convert.ToSingle(brakes && Controller.GetShipSpeed() > 1) * (float)velocity.Z + Convert.ToSingle(!brakes) * w.Power * 0.01f * -Controller.MoveIndicator.Z));
w.SetValue("Steer override", Math.Sign(Math.Round(Vector3D.Dot(w.WorldMatrix.Forward, Controller.WorldMatrix.Up), 2)) * Math.Sign(Vector3D.Dot(w.GetPosition() - avgWheelPosition, Controller.WorldMatrix.Forward)) * Controller.MoveIndicator.X + Controller.RollIndicator);
}
}
public void GetBestThrusters(Vector3D v1,
List <IMyTerminalBlock> thrustForwardList, List <IMyTerminalBlock> thrustBackwardList,
List <IMyTerminalBlock> thrustDownList, List <IMyTerminalBlock> thrustUpList,
List <IMyTerminalBlock> thrustLeftList, List <IMyTerminalBlock> thrustRightList,
out List <IMyTerminalBlock> thrustTowards, out List <IMyTerminalBlock> thrustAway
)
{
// Matrix or1;
double angle;
Vector3D vThrustAim;
Vector3D vNGN = v1;
vNGN.Normalize();
double cos45 = MathHelper.Sqrt2 * 0.5;
// thisProgram.sMasterReporting += "GBT: Checking cos45=" + cos45.ToString("0.00")+"\n";
// default selection to assign out parameters in main-line code
thrustTowards = thrustForwardList;
thrustAway = thrustBackwardList;
// thrustForwardList[0].Orientation.GetMatrix(out or1);
vThrustAim = thrustForwardList[0].WorldMatrix.Forward;
angle = vNGN.Dot(vThrustAim);
// thisProgram.sMasterReporting += "GBT:T F:Angle=" + angle.ToString("0.00") + "\n";
// thrustUpList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustUpList[0].WorldMatrix.Forward;
angle = vNGN.Dot(vThrustAim);
// thisProgram.sMasterReporting += "GBT:T U:Angle=" + angle.ToString("0.00") + "\n";
// thrustBackwardList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustBackwardList[0].WorldMatrix.Forward;
angle = vNGN.Dot(vThrustAim);
// thisProgram.sMasterReporting += "GBT:T B:Angle=" + angle.ToString("0.00") + "\n";
// thrustDownList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustDownList[0].WorldMatrix.Forward;
angle = vNGN.Dot(vThrustAim);
// thisProgram.sMasterReporting += "GBT:T D:Angle=" + angle.ToString("0.00") + "\n";
// thrustRightList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustRightList[0].WorldMatrix.Forward;
angle = vNGN.Dot(vThrustAim);
// thisProgram.sMasterReporting += "GBT:T R:Angle=" + angle.ToString("0.00") + "\n";
// thrustLeftList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustLeftList[0].WorldMatrix.Forward;
angle = vNGN.Dot(vThrustAim);
// thisProgram.sMasterReporting += "GBT:T L:Angle=" + angle.ToString("0.00") + "\n";
/*
* thrustDownList[0].CustomName = "thrust DN";
* thrustDownList[0].ShowOnHUD = true;
* thrustDownList[0].ShowInTerminal= true;
* thrustForwardList[0].CustomName = "thrust FW";
* thrustForwardList[0].ShowOnHUD = true;
* thrustForwardList[0].ShowInTerminal = true;
* thrustUpList[0].CustomName = "thrust Up";
* thrustUpList[0].ShowOnHUD = true;
* thrustUpList[0].ShowInTerminal = true;
* thrustBackwardList[0].CustomName = "thrust BK";
* thrustBackwardList[0].ShowOnHUD = true;
* thrustBackwardList[0].ShowInTerminal = true;
* thrustRightList[0].CustomName = "thrust RT";
* thrustRightList[0].ShowOnHUD = true;
* thrustRightList[0].ShowInTerminal = true;
* thrustLeftList[0].CustomName = "thrust LF";
* thrustLeftList[0].ShowOnHUD = true;
* thrustLeftList[0].ShowInTerminal = true;
*/
if (thrustForwardList.Count > 0)
{
// thrustForwardList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustForwardList[0].WorldMatrix.Forward;
angle = Math.Abs(vNGN.Dot(vThrustAim));
// thisProgram.sMasterReporting += "GBT: F:Angle="+angle.ToString("0.00") + "\n";
if (angle > cos45)
{
// thisProgram.Echo("GBT: Thrust fowrard");
thisProgram.sMasterReporting += "GBT: Thrust fowrard\n";
return;
}
}
if (thrustUpList.Count > 0)
{
// thrustUpList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustUpList[0].WorldMatrix.Forward;
angle = Math.Abs(vNGN.Dot(vThrustAim));
if (angle > cos45)
{
thisProgram.sMasterReporting += "GBT: Thrust UP\n";
// thisProgram.Echo("GBT: Thrust UP");
thrustTowards = thrustUpList;
thrustAway = thrustDownList;
return;
}
}
if (thrustBackwardList.Count > 0)
{
// thrustBackwardList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustBackwardList[0].WorldMatrix.Forward;
angle = Math.Abs(vNGN.Dot(vThrustAim));
if (angle > cos45)
{
// thisProgram.Echo("GBT: Thrust BACKWARD");
// thisProgram.sMasterReporting += "GBT: Thrust BACKWARD\n";
thrustTowards = thrustBackwardList;
thrustAway = thrustForwardList;
return;
}
}
if (thrustDownList.Count > 0)
{
// thrustDownList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustDownList[0].WorldMatrix.Forward;
angle = Math.Abs(vNGN.Dot(vThrustAim));
if (angle > cos45)
{
// thisProgram.Echo("GBT: Thrust DOWN");
// thisProgram.sMasterReporting += "GBT: Thrust DOWN\n";
thrustTowards = thrustDownList;
thrustAway = thrustUpList;
return;
}
}
if (thrustRightList.Count > 0)
{
// thrustRightList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustRightList[0].WorldMatrix.Forward;
angle = Math.Abs(vNGN.Dot(vThrustAim));
if (angle > cos45)
{
// thisProgram.Echo("GBT: Thrust RIGHT");
// thisProgram.sMasterReporting += "GBT: Thrust RIGHT\n";
thrustTowards = thrustRightList;
thrustAway = thrustLeftList;
return;
}
}
if (thrustLeftList.Count > 0)
{
// thrustLeftList[0].Orientation.GetMatrix(out or1);
// vThrustAim = or1.Forward;
vThrustAim = thrustLeftList[0].WorldMatrix.Forward;
angle = Math.Abs(vNGN.Dot(vThrustAim));
if (angle > cos45)
{
// thisProgram.Echo("GBT: Thrust LEFT");
// thisProgram.sMasterReporting += "GBT: Thrust LEFT\n";
thrustTowards = thrustLeftList;
thrustAway = thrustRightList;
return;
}
}
// thisProgram.Echo("GBT: Thrust DEFAULT");
thisProgram.sMasterReporting += "GBT: Thrust DEFAULT\n";
}
public static PlaneD FromPoints(Vector3D p1, Vector3D p2, Vector3D p3)
{
Vector3D Normal = (p2 - p1).Cross(p3 - p1);
Normal.Normalize();
return new PlaneD(Normal, -p1.Dot(Normal));
}
private void ImpactColorAssignments(int prevLod)
{
try
{
var ib = _backing.IndexBuffer[_lod];
for (int i = 0, j = 0; i < ib.Length; i += 3, j++)
{
var i0 = ib[i];
var i1 = ib[i + 1];
var i2 = ib[i + 2];
var v0 = _vertexBuffer[i0];
var v1 = _vertexBuffer[i1];
var v2 = _vertexBuffer[i2];
if (prevLod != _lod)
{
var lclPos = ((v0 + v1 + v2) / 3) - _matrix.Translation;
var normlclPos = Vector3D.Normalize(lclPos);
_preCalcNormLclPos[j] = normlclPos;
for (int c = 0; c < _triColorBuffer.Length; c++)
{
_triColorBuffer[c] = 0;
}
}
if (!ImpactsFinished)
{
for (int s = 0; s < 6; s++)
{
//// basically the same as for a sphere: offset by radius, except the radius will depend on the axis
//// if you already have the mesh generated, it's easy to get the vector from point - origin
//// when you have the vector, save the magnitude as the length (radius at that point), then normalize the vector
//// so it's length is 1, then multiply by length + wave offset you would need the original vertex points for each iteration
if (_localImpacts[s] == Vector3D.NegativeInfinity || _impactCnt[s] > SmallImpact + 1)
{
continue;
}
var dotOfNormLclImpact = Vector3D.Dot(_preCalcNormLclPos[i / 3], _localImpacts[s]);
var impactFactor = (((-0.69813170079773212 * dotOfNormLclImpact * dotOfNormLclImpact) - 0.87266462599716477) * dotOfNormLclImpact) + 1.5707963267948966;
var wavePosition = WaveMultiplier * _impactCnt[s];
var relativeToWavefront = Math.Abs(impactFactor - wavePosition);
if (impactFactor < wavePosition && relativeToWavefront >= 0 && relativeToWavefront < 0.25)
{
if (_impactCnt[s] != SmallImpact + 1)
{
_triColorBuffer[j] = 1;
}
else
{
_triColorBuffer[j] = 0;
}
break;
}
if ((impactFactor < wavePosition && relativeToWavefront >= -0.25 && relativeToWavefront < 0) || (relativeToWavefront > 0.25 && relativeToWavefront <= 0.5))
{
_triColorBuffer[j] = 0;
break;
}
}
}
}
}
catch (Exception ex) { Log.Line($"Exception in ImpactColorAssignments {ex}"); }
}
public string Update()
{
massOfShip = 0;
velocityTowardGravity = 0;
emergencyThrust = 0;
normalThrust = 0;
gravThrust = 0;
seaLevelAltitude = 0;
surfaceAltitude = 0;
lidarAltitude = 0;
landingAltitude = 0;
StatusBuilder.Clear();
StatusBuilder.Append($"\n OrbitalMode: {OrbitalMode}");
if (OrbitalMode == OrbitalOperation.Off)
{
return(StatusBuilder.ToString());
}
if (shipControllers.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
thrusters.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
emergencyThrusters.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
gravDriveNeg.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
gravDrivePos.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
vMass.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock) ||
downCameras.Any(de => de != de.CubeGrid.GetCubeBlock(de.Position)?.FatBlock))
{
OrbitalListNeedsBuilding = true;
}
if (!OrbitalListNeedsBuilding && shipControllers.Count > 0)
{
gravityVector = shipControllers[0].GetNaturalGravity();
normalGravity = gravityVector.Length() / 9.81;
if (thrusters.Count > 0 && thrusters[0].WorldMatrix.GetClosestDirection(gravityVector) == Base6Directions.Direction.Forward)
{
OrbitalListNeedsBuilding = true;
}
}
if (shipControllers.Count == 0 || OrbitalListNeedsBuilding)
{
foreach (IMyThrust de in thrusters)
{
de?.SetValueFloat("Override", 0);
}
foreach (IMyThrust de in emergencyThrusters)
{
de?.SetValueFloat("Override", 0);
}
foreach (IMyGravityGenerator de in gravDrivePos)
{
de.GravityAcceleration = 0;
}
foreach (IMyGravityGenerator de in gravDriveNeg)
{
de.GravityAcceleration = 0;
}
BuildOrbitList();
}
if (shipControllers.Count == 0)
{
//cannot go on.
StatusBuilder.Append("\nOrbital: Error no shipController");
return(StatusBuilder.ToString());
}
massOfShip = shipControllers[0].CalculateShipMass().PhysicalMass;
shipVector = shipControllers[0].GetShipVelocities().LinearVelocity;
shipToGravityVector = VectorProjection(shipVector, gravityVector);
velocityTowardGravity = ((int)(-1 * Math.Sign(gravityVector.Dot(shipVector)))) * shipToGravityVector.Length();
StatusBuilder.Append($"\n Mass : {DisplayLargeNumber(massOfShip)} kg");
StatusBuilder.Append($"\n Rate of Change: {velocityTowardGravity.ToString("0.00")} m/s");
shipControllers[0].TryGetPlanetElevation(MyPlanetElevation.Sealevel, out seaLevelAltitude);
foreach (IMyShipController de in shipControllers)
{
de.TryGetPlanetElevation(MyPlanetElevation.Surface, out tempDouble);
if (tempDouble > surfaceAltitude)
{
surfaceAltitude = tempDouble;
}
}
//thrusters
foreach (IMyThrust de in thrusters)
{
if (de.IsWorking)
{
normalThrust += de.MaxEffectiveThrust;
}
}
if (normalThrust == 0)
{
normalThrust = 1;
}
foreach (IMyThrust de in emergencyThrusters)
{
emergencyThrust += de.MaxEffectiveThrust;
}
if (GravDriveOn)
{
gravThrust = 490500 * (gravDriveNeg.Count + gravDrivePos.Count);
}
thrustNeeded = (float)(massOfShip * normalGravity * 9.81);
StatusBuilder.Append($"\n Thrust needed : {DisplayLargeNumber(thrustNeeded)} N");
//Decision point
if (normalGravity == 0)
{
foreach (IMyThrust de in thrusters)
{
de.SetValueFloat("Override", 0);
}
foreach (IMyThrust de in emergencyThrusters)
{
de.SetValueFloat("Override", 0);
de.Enabled = false;
}
foreach (IMyGravityGenerator de in gravDriveNeg)
{
de.GravityAcceleration = 0;
}
foreach (IMyGravityGenerator de in gravDrivePos)
{
de.GravityAcceleration = 0;
}
StatusBuilder.Append($"\n Not in gravity well");
return(StatusBuilder.ToString());
}
effectiveGravDriveThrust = gravThrust * MathHelper.Clamp(1 - 2 * normalGravity, 0f, 1f);
maxEffectiveThrust = effectiveGravDriveThrust + normalThrust;
tempAdjustmentGravity = MathHelper.Clamp(normalGravity * 9.31, 0, VelocityLimit / 2);
availableThrust = Math.Max(maxEffectiveThrust - thrustNeeded, 0);
targetVelocity = 0;
if (OrbitalMode == OrbitalOperation.AltitudeMode || OrbitalMode == OrbitalOperation.HoverMode)
{
foreach (IMyCameraBlock de in downCameras)
{
de.EnableRaycast = true;
}
lidarAltitude = LidarRange(downCameras, 0, 0);
if (lidarAltitude < surfaceAltitude)
{
landingAltitude = lidarAltitude;
}
else
{
landingAltitude = surfaceAltitude;
}
if (OrbitalMode == OrbitalOperation.HoverMode)
{
tempDouble = HoverTarget + seaLevelAltitude - landingAltitude;
}
else
{
tempDouble = AltitudeTarget;
}
if (seaLevelAltitude < tempDouble - AltitudeBuffer)
{
targetVelocity = MathHelper.Clamp(Math.Sqrt(tempAdjustmentGravity * Math.Abs(tempDouble - seaLevelAltitude - velocityTowardGravity)), 0, VelocityLimit);
}
else if (seaLevelAltitude > tempDouble + AltitudeBuffer)
{
if (landingAltitude < (HeightOffset + 100 + velocityTowardGravity * velocityTowardGravity / tempAdjustmentGravity))
{
//landing
targetVelocity = -Math.Max(Math.Sqrt(Math.Abs(MathHelper.Clamp((landingAltitude - HeightOffset - 12.5), 0d, 100) * tempAdjustmentGravity)), 1d);
}
else
{
targetVelocity = -MathHelper.Clamp(Math.Sqrt(0.7 * tempAdjustmentGravity * Math.Abs(seaLevelAltitude - tempDouble + velocityTowardGravity)), 0, VelocityLimit);
}
}
}
//could just do else here
if (OrbitalMode == OrbitalOperation.GravityMode)
{
if (Math.Round(normalGravity, 2) > Math.Round(GravityTarget, 2))
{
if (VelocityLimit > tempAdjustmentGravity * 10)
{
targetVelocity = 10 * tempAdjustmentGravity;
}
else
{
targetVelocity = VelocityLimit;
}
}
else if (Math.Round(normalGravity, 2) < Math.Round(GravityTarget, 2))
{
targetVelocity = -VelocityLimit;
}
}
StatusBuilder.Append($"\n request velocity {targetVelocity.ToString("0.00")} m/s");
requestedThrust = (targetVelocity - velocityTowardGravity) * massOfShip + thrustNeeded;
//gravDrive
if (GravDriveOn)
{
thrustApplied = Math.Sign(requestedThrust) * Math.Min(Math.Abs(requestedThrust), effectiveGravDriveThrust);
requestedThrust -= Math.Min(Math.Abs(requestedThrust), effectiveGravDriveThrust);
}
requestedGravity = (float)((thrustApplied / (MathHelper.Clamp(1 - 2 * normalGravity, 0f, 1f)) / 50000) / (gravDrivePos.Count + gravDriveNeg.Count));
if (requestedGravity < 0 || float.IsNaN(requestedGravity))
{
requestedGravity = 0;
}
foreach (IMyGravityGenerator de in gravDrivePos)
{
de.GravityAcceleration = requestedGravity;
}
foreach (IMyGravityGenerator de in gravDriveNeg)
{
de.GravityAcceleration = -requestedGravity;
}
//Normal Drive
if (thrusters.Count > 0 && requestedThrust > 0)
{
thrustApplied = Math.Min(requestedThrust, normalThrust);
requestedThrust -= thrustApplied;
}
normalThrustPerThruster = (float)(100 * thrustApplied / normalThrust);
if (normalThrustPerThruster < 1.0001)
{
normalThrustPerThruster = 1.0001f;
}
foreach (IMyThrust de in thrusters)
{
de.SetValueFloat("Override", normalThrustPerThruster);
}
if (OrbitalMode == OrbitalOperation.GravityMode && normalGravity > GravityEmergency)
{
// Emergency mode
StatusBuilder.Append("\n Emergency thrust engaged");
foreach (IMyThrust de in emergencyThrusters)
{
de.Enabled = true;
de.SetValueFloat("Override", 100);
}
}
else if (OrbitalMode == OrbitalOperation.GravityMode && normalGravity < GravityEmergency)
{
foreach (IMyThrust de in emergencyThrusters)
{
de.Enabled = false;
de.SetValueFloat("Override", 0);
}
}
if (OrbitalMode == OrbitalOperation.AltitudeMode || OrbitalMode == OrbitalOperation.HoverMode)
{
StatusBuilder.Append($"\n Altitude = {seaLevelAltitude.ToString("0")} m.");
if (OrbitalMode == OrbitalOperation.AltitudeMode)
{
StatusBuilder.Append($" Target = {AltitudeTarget.ToString("0")} m.");
}
else
{
StatusBuilder.Append($" Hover at = {HoverTarget.ToString("0")} m.");
}
StatusBuilder.Append($"\n Ground level = {surfaceAltitude.ToString("0")} m.");
StatusBuilder.Append($"\n Lidar Altitude = {lidarAltitude.ToString("0")} m.");
}
if (OrbitalMode == OrbitalOperation.GravityMode)
{
StatusBuilder.Append($"\n Current Orbit = {normalGravity.ToString("0.000")} g.");
StatusBuilder.Append($"\n Target Orbit = {GravityTarget.ToString("0.000")} g.");
}
StatusBuilder.Append($"\n Lift: Normal: {DisplayLargeNumber((float)(normalThrust / (normalGravity * 9.81)))} kg.");
StatusBuilder.Append($"\n GravDrive: {DisplayLargeNumber((float)(effectiveGravDriveThrust / (normalGravity * 9.81)))} kg.");
StatusBuilder.Append($"\n Emergency: {DisplayLargeNumber((float)(emergencyThrust / (normalGravity * 9.81)))} kg.");
return(StatusBuilder.ToString());
}
/// <summary>
/// Returns the acute angle between two vectors.
/// </summary>
public static double AngleBetween(this Vector3D first, Vector3D second)
{
return(Math.Acos(first.Dot(second) / (first.Length() * second.Length())));
}
public bool AreNormalsEqual(Vector3D normal1, Vector3D normal2)
{
return(Vector3D.Dot(normal1, normal2) > normalDotThreshold);
}
public static PlaneD FromPointNormal(Vector3D point, PlaneD normal)
{
return new PlaneD(normal.Normal, -point.Dot(normal.Normal));
}
/// <summary>
/// Export the HMesh as a number of meshes, split into a number of subregions.
/// If the parameter used material is provided, the mesh only contains submeshes with geometry and the submesh index
/// is added to the used material.
/// Is a mesh does not contain any vertices it is skipped.
/// </summary>
public List <Mesh> ExportSplit(Vector3i axisSplit, List <List <int> > usedMaterials = null, double sharpEdgeAngle = 360, IndexFormat indexFormat = IndexFormat.UInt16)
{
var bounds = ComputeBoundsD();
bounds.extents += Vector3D.one * 0.000001; // add delta size to ensure no vertex is on bounds
var resList = new List <Mesh>();
// Enumerate vertices
for (int i = 0; i < vertices.Count; i++)
{
vertices[i].label = i;
}
var maxFaceLabel = 0;
foreach (var face in faces)
{
maxFaceLabel = Mathf.Max(maxFaceLabel, face.label);
}
int clusterCount = 0;
MarkSharpEdges(sharpEdgeAngle);
var faceClusters = SeparateMeshByGeometry(out clusterCount);
Debug.Log("Face clusters: " + clusterCount);
var remap = new int[vertices.Count];
for (var i = 0; i < axisSplit[0]; i++)
{
double minX = bounds.min.x + i * (bounds.size.x / axisSplit[0]);
double maxX = bounds.min.x + (i + 1) * (bounds.size.x / axisSplit[0]);
for (var j = 0; j < axisSplit[1]; j++)
{
double minY = bounds.min.y + j * (bounds.size.y / axisSplit[1]);
double maxY = bounds.min.y + (j + 1) * (bounds.size.y / axisSplit[1]);
for (var k = 0; k < axisSplit[2]; k++)
{
double minZ = bounds.min.z + k * (bounds.size.z / axisSplit[2]);
double maxZ = bounds.min.z + (k + 1) * (bounds.size.z / axisSplit[2]);
// DebugExt.DrawBox(new Vector3D(minX, minY, minZ).ToVector3(), new Vector3D(maxX, maxY, maxZ).ToVector3(),Color.white, 10);
var min = new Vector3D(minX, minY, minZ);
var max = new Vector3D(maxX, maxY, maxZ);
var res = new Mesh();
res.indexFormat = indexFormat;
res.name = "HMesh_" + i + "," + j + "," + k;
var vertexArray = new List <Vector3>();
var normalArray = new List <Vector3>();
var uv1 = new List <Vector2>();
var uv2 = new List <Vector2>();
res.subMeshCount = maxFaceLabel + 1;
List <Face> facesInRegion = new List <Face>();
foreach (var face in faces)
{
var center = face.GetCenter();
if (Vector3D.AllLessThan(min, center) && Vector3D.AllLessEqualThan(center, max))
{
facesInRegion.Add(face);
}
}
var triangles = new List <List <int> >();
for (int ii = 0; ii <= maxFaceLabel; ii++)
{
triangles.Add(new List <int>());
}
for (int clusterIndex = 0; clusterIndex < clusterCount; clusterIndex++)
{
// clear remap
for (int x = 0; x < remap.Length; x++)
{
remap[x] = -1;
}
for (var faceLabel = 0; faceLabel <= maxFaceLabel; faceLabel++)
{
foreach (var face in facesInRegion)
{
if (faceClusters[face.id] != clusterIndex)
{
continue;
}
if (face.label == faceLabel)
{
if (face.NoEdges() != 3)
{
Debug.LogError("Only triangles supported. Was " + face.NoEdges() + "-gon");
continue;
}
var he = face.halfedge;
var first = true;
while (he != face.halfedge || first)
{
var indexOfVertex = he.vert.label;
var indexOfVertexRemapped = remap[indexOfVertex];
if (indexOfVertexRemapped == -1)
{
indexOfVertexRemapped = vertexArray.Count;
remap[indexOfVertex] = indexOfVertexRemapped;
vertexArray.Add(vertices[indexOfVertex].position);
uv1.Add(vertices[indexOfVertex].uv1);
uv2.Add(vertices[indexOfVertex].uv2);
// compute normal
Vector3D n = Vector3D.zero;
int count = 0;
foreach (var vertHe in he.vert.CirculateAllIngoing())
{
if (faceClusters[vertHe.face.id] == clusterIndex)
{
double angle = Vector3D.Angle(-vertHe.GetDirection(), vertHe.next.GetDirection());
n += angle * vertHe.face.GetNormal();
count++;
}
}
if (n.sqrMagnitude <= 0 || double.IsNaN(Vector3D.Dot(n, n)))
{
Debug.LogWarning("Cannot compute normal n is " + n.ToString("R") + " edges of vertex " + he.vert.Circulate().Count + " same cluster " + count + " " + he.face.ToString());
foreach (var vertHe in he.vert.CirculateAllIngoing())
{
if (faceClusters[vertHe.face.id] == clusterIndex)
{
Debug.Log(vertHe.face.ToString());
}
}
n = new Vector3D(0, 1, 0);
}
else
{
n.Normalize();
}
normalArray.Add(n.ToVector3());
}
triangles[faceLabel].Add(indexOfVertexRemapped);
he = he.next;
first = false;
}
}
}
}
}
if (vertexArray.Count == 0)
{
// empty mesh - skip
continue;
}
resList.Add(res);
if (vertexArray.Count > 65000)
{
Debug.LogWarning("Vertex count was " + vertexArray.Count);
}
res.vertices = vertexArray.ToArray();
res.uv = uv1.ToArray();
res.uv2 = uv2.ToArray();
res.normals = normalArray.ToArray();
// add mesh indices
// if usedMaterials exists then filter out any empty submesh
List <int> materialIndices = null;
if (usedMaterials != null)
{
materialIndices = new List <int>();
usedMaterials.Add(materialIndices);
}
for (int ii = 0; ii < triangles.Count; ii++)
{
if (materialIndices != null)
{
if (triangles[ii].Count > 0)
{
res.SetIndices(triangles[ii].ToArray(), MeshTopology.Triangles, materialIndices.Count, true);
materialIndices.Add(ii);
}
}
else
{
res.SetIndices(triangles[ii].ToArray(), MeshTopology.Triangles, ii, true);
}
}
res.RecalculateBounds();
}
}
}
return(resList);
}
/// <summary>
/// Rotates a 3D coordinate system to match a specified normal.
/// </summary>
/// <param name="uOld">Old u.</param>
/// <param name="vOld">Old v.</param>
/// <param name="normalNew">The normal to match.</param>
/// <param name="uNew">New u.</param>
/// <param name="vNew">New v.</param>
public static void RotateCoordinateSystem(Vector3D uOld, Vector3D vOld, Vector3D normalNew,
out Vector3D uNew, out Vector3D vNew)
{
Vector3D normalOld = uOld.Cross(vOld);
float normalsDot =(float) normalOld.Dot(normalNew);
if (normalsDot <= -1.0f)
{
uNew = uOld * -1;
vNew = vOld * -1; ;
}
else
{
Vector3D perpOld = normalNew - normalsDot * normalOld;
Vector3D dPerp = 1.0f / (1.0f + normalsDot) * (normalOld + normalNew);
uNew = uOld - dPerp * uOld.Dot(perpOld);
vNew = vOld - dPerp * vOld.Dot(perpOld);
}
}
// Determine whether the given ray intersects this box. If so, returns
// the parametric value of the point of first intersection; otherwise
// returns null.
public double?Intersects(ref RayD ray)
{
MatrixD R = Matrix.CreateFromQuaternion(Orientation);
Vector3D TOrigin = Center - ray.Position;
double t_min = -double.MaxValue;
double t_max = double.MaxValue;
// X-case
double axisDotOrigin = Vector3D.Dot(R.Right, TOrigin);
double axisDotDir = Vector3D.Dot(R.Right, ray.Direction);
if (axisDotDir >= -RAY_EPSILON && axisDotDir <= RAY_EPSILON)
{
if ((-axisDotOrigin - HalfExtent.X) > 0.0 || (-axisDotOrigin + HalfExtent.X) < 0.0f)
{
return(null);
}
}
else
{
double t1 = (axisDotOrigin - HalfExtent.X) / axisDotDir;
double t2 = (axisDotOrigin + HalfExtent.X) / axisDotDir;
if (t1 > t2)
{
double temp = t1;
t1 = t2;
t2 = temp;
}
if (t1 > t_min)
{
t_min = t1;
}
if (t2 < t_max)
{
t_max = t2;
}
if (t_max < 0.0f || t_min > t_max)
{
return(null);
}
}
// Y-case
axisDotOrigin = Vector3.Dot(R.Up, TOrigin);
axisDotDir = Vector3.Dot(R.Up, ray.Direction);
if (axisDotDir >= -RAY_EPSILON && axisDotDir <= RAY_EPSILON)
{
if ((-axisDotOrigin - HalfExtent.Y) > 0.0 || (-axisDotOrigin + HalfExtent.Y) < 0.0f)
{
return(null);
}
}
else
{
double t1 = (axisDotOrigin - HalfExtent.Y) / axisDotDir;
double t2 = (axisDotOrigin + HalfExtent.Y) / axisDotDir;
if (t1 > t2)
{
double temp = t1;
t1 = t2;
t2 = temp;
}
if (t1 > t_min)
{
t_min = t1;
}
if (t2 < t_max)
{
t_max = t2;
}
if (t_max < 0.0f || t_min > t_max)
{
return(null);
}
}
// Z-case
axisDotOrigin = Vector3.Dot(R.Forward, TOrigin);
axisDotDir = Vector3.Dot(R.Forward, ray.Direction);
if (axisDotDir >= -RAY_EPSILON && axisDotDir <= RAY_EPSILON)
{
if ((-axisDotOrigin - HalfExtent.Z) > 0.0 || (-axisDotOrigin + HalfExtent.Z) < 0.0f)
{
return(null);
}
}
else
{
double t1 = (axisDotOrigin - HalfExtent.Z) / axisDotDir;
double t2 = (axisDotOrigin + HalfExtent.Z) / axisDotDir;
if (t1 > t2)
{
double temp = t1;
t1 = t2;
t2 = temp;
}
if (t1 > t_min)
{
t_min = t1;
}
if (t2 < t_max)
{
t_max = t2;
}
if (t_max < 0.0f || t_min > t_max)
{
return(null);
}
}
return(t_min);
}
private static float GetIdealSliceSpacing(IVolumeHeader volumeHeader, Vector3D unitSpacingAxis)
{
// the ideal spacing is simply the diagonal of the voxel projected on to the spacing axis
// any larger than this value, and it becomes possible for an entire voxel to fit in between two consecutive output locations (i.e. missed for interpolation)
// any smaller than this value, and some voxels will have two or more output locations within their bounds
return Math.Abs(unitSpacingAxis.Dot(volumeHeader.RotateToPatientOrientation(volumeHeader.VoxelSpacing)));
}
void doModeGoTarget()
{
StatusLog("clear", textPanelReport);
StatusLog(moduleName + ":Going Target!", textPanelReport);
// StatusLog(moduleName + ":GT: current_state=" + current_state.ToString(), textPanelReport);
// bWantFast = true;
Echo("Going Target: state=" + current_state.ToString());
if (NAVTargetName != "")
{
Echo(NAVTargetName);
}
string sNavDebug = "";
sNavDebug += "GT:S=" + current_state;
// sNavDebug += " MinE=" + NAVGravityMinElevation;
// ResetMotion();
if (current_state == 0)
{
ResetTravelMovement();
// sStartupError+="\nStart movemenet: ArrivalMode="+NAVArrivalMode+" State="+NAVArrivalState;
if ((craft_operation & CRAFT_MODE_SLED) > 0)
{
bSled = true;
if (shipSpeedMax > 45)
{
shipSpeedMax = 45;
}
}
else
{
bSled = false;
}
if ((craft_operation & CRAFT_MODE_ROTOR) > 0)
{
bRotor = true;
if (shipSpeedMax > 15)
{
shipSpeedMax = 15;
}
}
else
{
bRotor = false;
}
if ((craft_operation & CRAFT_MODE_WHEEL) > 0)
{
bWheels = true;
// if (shipSpeedMax > 15) shipSpeedMax = 15;
}
else
{
bWheels = false;
}
GyroControl.SetRefBlock(shipOrientationBlock);
// TODO: Put a timer on this so it's not done Update1
double elevation = 0;
((IMyShipController)shipOrientationBlock).TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation);
if (!bSled && !bRotor)
{ // if flying ship
// make sure set to default
if (NAVGravityMinElevation < 0)
{
NAVGravityMinElevation = 75; // for EFM getting to target 'arrived' radius
}
// NAVGravityMinElevation = (float)shipSpeedMax*2.5f;
}
if (bValidNavTarget)
{
if (elevation > shipDim.HeightInMeters())
{
current_state = 150;
}
else
{
current_state = 160;
}
}
else
{
setMode(MODE_ATTENTION);
}
bWantFast = true;
}
else if (current_state == 150)
{
bWantFast = true;
if (dGravity > 0)
{
double elevation = 0;
((IMyShipController)shipOrientationBlock).TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation);
sNavDebug += " E=" + elevation.ToString("0.0");
float fSaveAngle = minAngleRad;
minAngleRad = 0.1f;
Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity();
bool bAligned = GyroMain("", grav, shipOrientationBlock);
sNavDebug += " Aligned=" + bAligned.ToString();
Echo("bAligned=" + bAligned.ToString());
minAngleRad = fSaveAngle;
if (bAligned || elevation < shipDim.HeightInMeters() * 2)
{
gyrosOff();
if (NAVGravityMinElevation > 0)
{
current_state = 155;
}
else
{
current_state = 160;
}
}
}
else
{
current_state = 160;
}
}
else if (current_state == 151)
{
bWantFast = true;
if (dGravity > 0 || btmWheels)
{
double elevation = 0;
((IMyShipController)shipOrientationBlock).TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation);
sNavDebug += " E=" + elevation.ToString("0.0");
float fSaveAngle = minAngleRad;
minAngleRad = 0.1f;
Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity();
bool bAligned = GyroMain("", grav, shipOrientationBlock);
sNavDebug += " Aligned=" + bAligned.ToString();
Echo("bAligned=" + bAligned.ToString());
minAngleRad = fSaveAngle;
if (bAligned || elevation < shipDim.HeightInMeters() * 2)
{
gyrosOff();
if (NAVGravityMinElevation > 0)
{
current_state = 155;
}
else
{
current_state = 160;
}
}
else
{
current_state = 150; // try again to be aligned.
}
}
else
{
current_state = 160;
}
}
else if (current_state == 155)
{ // for use in gravity: aim at location using yaw only
bWantFast = true;
if (bWheels)
{
current_state = 160;
return;
}
if (dGravity > 0)
{
Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity();
bool bAligned = GyroMain("", grav, shipOrientationBlock);
sNavDebug += " Aligned=" + bAligned.ToString();
double yawangle = -999;
yawangle = CalculateYaw(vNavTarget, shipOrientationBlock);
bool bAimed = Math.Abs(yawangle) < 0.1; // NOTE: 2x allowance
Echo("yawangle=" + yawangle.ToString());
sNavDebug += " Yaw=" + yawangle.ToString("0.00");
Echo("bAimed=" + bAimed.ToString() + " bAligned=" + bAligned.ToString());
if (!bAimed)
{
if (btmRotor)
{
Echo("Rotor");
DoRotorRotate(yawangle);
}
else // use for both sled and flight
{
DoRotate(yawangle, "Yaw");
}
}
if (bAligned && bAimed)
{
gyrosOff();
current_state = 160;
}
else if (bAligned && Math.Abs(yawangle) < 0.5)
{
float atmo;
float hydro;
float ion;
calculateHoverThrust(thrustForwardList, out atmo, out hydro, out ion);
atmo += 1;
hydro += 1;
ion += 1;
powerUpThrusters(thrustForwardList, atmo, thrustatmo);
powerUpThrusters(thrustForwardList, hydro, thrusthydro);
powerUpThrusters(thrustForwardList, ion, thrustion);
}
else
{
powerDownThrusters(thrustForwardList);
}
}
else
{
current_state = 160;
}
}
else if (current_state == 156)
{
// realign gravity
bWantFast = true;
Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity();
bool bAimed = GyroMain("", grav, shipOrientationBlock);
if (bAimed)
{
gyrosOff();
current_state = 160;
}
}
else if (current_state == 160)
{ // 160 move to Target
Echo("Moving to Target");
Vector3D vTargetLocation = vNavTarget;
Vector3D vVec = vTargetLocation - shipOrientationBlock.GetPosition();
double distance = vVec.Length();
Echo("distance=" + niceDoubleMeters(distance));
Echo("velocity=" + velocityShip.ToString("0.00"));
StatusLog("clear", sledReport);
string sTarget = "Moving to Target";
if (NAVTargetName != "")
{
sTarget = "Moving to " + NAVTargetName;
}
StatusLog(sTarget + "\nD:" + niceDoubleMeters(distance) + " V:" + velocityShip.ToString(velocityFormat), sledReport);
StatusLog(sTarget + "\nDistance: " + niceDoubleMeters(distance) + "\nVelocity: " + niceDoubleMeters(velocityShip) + "/s", textPanelReport);
if (bGoOption && (distance < arrivalDistanceMin))
{
current_state = 500;
Echo("we have arrived");
bWantFast = true;
return;
}
// debugGPSOutput("TargetLocation", vTargetLocation);
bool bDoTravel = true;
if (NAVGravityMinElevation > 0 && dGravity > 0)
{
double elevation = 0;
MyShipVelocities mysSV = ((IMyShipController)shipOrientationBlock).GetShipVelocities();
Vector3D lv = mysSV.LinearVelocity;
var upVec = ((IMyShipController)shipOrientationBlock).WorldMatrix.Up;
var vertVel = Vector3D.Dot(lv, upVec);
// Echo("LV=" + Vector3DToString(lv));
// sNavDebug += " LV=" + Vector3DToString(lv);
// sNavDebug += " vertVel=" + vertVel.ToString("0.0");
// sNavDebug += " Hvel=" + lv.Y.ToString("0.0");
// NOTE: Elevation is only updated by game every 30? ticks. so it can be WAY out of date based on movement
((IMyShipController)shipOrientationBlock).TryGetPlanetElevation(MyPlanetElevation.Surface, out elevation);
sNavDebug += " E=" + elevation.ToString("0.0");
sNavDebug += " V=" + velocityShip.ToString("0.00");
Echo("Elevation=" + elevation.ToString("0.0"));
Echo("MinEle=" + NAVGravityMinElevation.ToString("0.0"));
// double stopD = calculateStoppingDistance(thrustUpList, velocityShip, dGravity);
double stopD = 0;
if (vertVel < 0)
{
stopD = calculateStoppingDistance(thrustUpList, Math.Abs(vertVel), dGravity);
}
double maxStopD = calculateStoppingDistance(thrustUpList, fMaxWorldMps, dGravity);
float atmo;
float hydro;
float ion;
calculateHoverThrust(thrustUpList, out atmo, out hydro, out ion);
// sNavDebug += " SD=" + stopD.ToString("0");
if (
// !bSled && !bRotor &&
NAVGravityMinElevation > 0)
{
if (
vertVel < -0.5 && // we are going downwards
(elevation - stopD * 2) < NAVGravityMinElevation)
{ // too low. go higher
// Emergency thrust
sNavDebug += " EM UP!";
Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity();
bool bAligned = GyroMain("", grav, shipOrientationBlock);
powerUpThrusters(thrustUpList, 100);
bDoTravel = false;
bWantFast = true;
}
else if (elevation < NAVGravityMinElevation)
{
// push upwards
atmo += Math.Min(5f, (float)shipSpeedMax);
hydro += Math.Min(5f, (float)shipSpeedMax);
ion += Math.Min(5f, (float)shipSpeedMax);
sNavDebug += " UP! A" + atmo.ToString("0.00"); // + " H"+hydro.ToString("0.00") + " I"+ion.ToString("0.00");
//powerUpThrusters(thrustUpList, 100);
powerUpThrusters(thrustUpList, atmo, thrustatmo);
powerUpThrusters(thrustUpList, hydro, thrusthydro);
powerUpThrusters(thrustUpList, ion, thrustion);
}
else if (elevation > (maxStopD + NAVGravityMinElevation * 1.25))
{
// if we are higher than maximum possible stopping distance, go down fast.
sNavDebug += " SUPERHIGH";
// Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity();
// bool bAligned = GyroMain("", grav, shipOrientationBlock);
powerDownThrusters(thrustUpList, thrustAll, true);
Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity();
bool bAligned = GyroMain("", grav, shipOrientationBlock);
if (!bAligned)
{
bWantFast = true;
bDoTravel = false;
}
// powerUpThrusters(thrustUpList, 1f);
}
else if (
elevation > NAVGravityMinElevation * 2 // too high
// && ((elevation-stopD)>NAVGravityMinElevation) // we can stop in time.
// && velocityShip < shipSpeedMax * 1.1 // to fast in any direction
// && Math.Abs(lv.X) < Math.Min(25, shipSpeedMax) // not too fast
// && Math.Abs(lv.Y) < Math.Min(25, shipSpeedMax) // not too fast downwards (or upwards)
)
{ // too high
sNavDebug += " HIGH";
//DOWN! A" + atmo.ToString("0.00");// + " H" + hydro.ToString("0.00") + " I" + ion.ToString("0.00");
if (vertVel > 2) // going up
{ // turn off thrusters.
sNavDebug += " ^";
powerDownThrusters(thrustUpList, thrustAll, true);
}
else if (vertVel < -0.5) // going down
{
sNavDebug += " v";
if (vertVel > (-Math.Min(15, shipSpeedMax)))
{
// currently descending at less than desired
atmo -= Math.Max(25f, Math.Min(5f, (float)velocityShip / 2));
hydro -= Math.Max(25f, Math.Min(5f, (float)velocityShip / 2));
ion -= Math.Max(25f, Math.Min(5f, (float)velocityShip / 2));
sNavDebug += " DOWN! A" + atmo.ToString("0.00");// + " H" + hydro.ToString("0.00") + " I" + ion.ToString("0.00");
// bDoTravel = false;
}
else
{
// we are descending too fast.
atmo += Math.Max(100f, Math.Min(5f, (float)velocityShip / 2));
hydro += Math.Max(100f, Math.Min(5f, (float)velocityShip / 2));
ion += Math.Max(100f, Math.Min(5f, (float)velocityShip / 2));
sNavDebug += " 2FAST! A" + atmo.ToString("0.00");// + " H" + hydro.ToString("0.00") + " I" + ion.ToString("0.00");
Vector3D grav = (shipOrientationBlock as IMyShipController).GetNaturalGravity();
bool bAligned = GyroMain("", grav, shipOrientationBlock);
if (!bAligned)
{
bWantFast = true;
bDoTravel = false;
}
// bDoTravel = false;
}
}
else
{
sNavDebug += " -";
atmo -= 5;
hydro -= 5;
ion -= 5;
}
powerUpThrusters(thrustUpList, atmo, thrustatmo);
powerUpThrusters(thrustUpList, hydro, thrusthydro);
powerUpThrusters(thrustUpList, ion, thrustion);
}
else
{
// normal hover
powerDownThrusters(thrustUpList);
}
}
}
if (bDoTravel)
{
Echo("Do Travel");
doTravelMovement(vTargetLocation, (float)arrivalDistanceMin, 500, 300);
}
else
{
powerDownThrusters(thrustForwardList);
}
}
else if (current_state == 300)
{ // collision detection
bWantFast = true;
Vector3D vTargetLocation = vNavTarget;
ResetTravelMovement();
calcCollisionAvoid(vTargetLocation);
// current_state = 301; // testing
current_state = 320;
}
else if (current_state == 301)
{
// just hold this state
bWantFast = false;
}
else if (current_state == 320)
{
// Vector3D vVec = vAvoid - shipOrientationBlock.GetPosition();
// double distanceSQ = vVec.LengthSquared();
Echo("Primary Collision Avoid");
StatusLog("clear", sledReport);
StatusLog("Collision Avoid", sledReport);
StatusLog("Collision Avoid", textPanelReport);
doTravelMovement(vAvoid, 5.0f, 160, 340);
}
else if (current_state == 340)
{ // secondary collision
if (
lastDetectedInfo.Type == MyDetectedEntityType.LargeGrid ||
lastDetectedInfo.Type == MyDetectedEntityType.SmallGrid
)
{
current_state = 345;
}
else if (lastDetectedInfo.Type == MyDetectedEntityType.Asteroid
)
{
current_state = 350;
}
else
{
current_state = 300; // setMode(MODE_ATTENTION);
}
bWantFast = true;
}
else if (current_state == 345)
{
// we hit a grid. align to it
Vector3D[] corners = new Vector3D[BoundingBoxD.CornerCount];
BoundingBoxD bbd = lastDetectedInfo.BoundingBox;
bbd.GetCorners(corners);
GridUpVector = PlanarNormal(corners[3], corners[4], corners[7]);
GridRightVector = PlanarNormal(corners[0], corners[1], corners[4]);
bWantFast = true;
current_state = 348;
}
else if (current_state == 348)
{
bWantFast = true;
if (GyroMain("up", GridUpVector, shipOrientationBlock))
{
current_state = 349;
}
}
else if (current_state == 349)
{
bWantFast = true;
if (GyroMain("right", GridRightVector, shipOrientationBlock))
{
current_state = 350;
}
}
else if (current_state == 350)
{
// initEscapeScan(bCollisionWasSensor, !bCollisionWasSensor);
initEscapeScan(bCollisionWasSensor);
ResetTravelMovement();
dtNavStartShip = DateTime.Now;
current_state = 360;
bWantFast = true;
}
else if (current_state == 360)
{
StatusLog("Collision Avoid\nScan for escape route", textPanelReport);
DateTime dtMaxWait = dtNavStartShip.AddSeconds(5.0f);
DateTime dtNow = DateTime.Now;
if (DateTime.Compare(dtNow, dtMaxWait) > 0)
{
setMode(MODE_ATTENTION);
doTriggerMain();
return;
}
if (scanEscape())
{
Echo("ESCAPE!");
current_state = 380;
}
bWantMedium = true;
// bWantFast = true;
}
else if (current_state == 380)
{
StatusLog("Collision Avoid Travel", textPanelReport);
Echo("Escape Collision Avoid");
doTravelMovement(vAvoid, 1f, 160, 340);
}
else if (current_state == 500)
{ // we have arrived at target
/*
* // check for more nav commands
* if(wicoNavCommands.Count>0)
* {
* wicoNavCommands.RemoveAt(0);
* }
* if(wicoNavCommands.Count>0)
* {
* // another command
* wicoNavCommandProcessNext();
* }
* else
*/
{
StatusLog("clear", sledReport);
StatusLog("Arrived at Target", sledReport);
StatusLog("Arrived at Target", textPanelReport);
sNavDebug += " ARRIVED!";
ResetMotion();
bValidNavTarget = false; // we used this one up.
// float range = RangeToNearestBase() + 100f + (float)velocityShip * 5f;
antennaMaxPower(false);
SensorsSleepAll();
// sStartupError += "Finish WP:" + wicoNavCommands.Count.ToString()+":"+NAVArrivalMode.ToString();
// set to desired mode and state
setMode(NAVArrivalMode);
current_state = NAVArrivalState;
// set up defaults for next run (in case they had been changed)
NAVArrivalMode = MODE_ARRIVEDTARGET;
NAVArrivalState = 0;
NAVTargetName = "";
bGoOption = true;
// setMode(MODE_ARRIVEDTARGET);
if (NAVEmulateOld)
{
var tList = GetBlocksContains <IMyTerminalBlock>("NAV:");
for (int i1 = 0; i1 < tList.Count(); i1++)
{
// don't want to get blocks that have "NAV:" in customdata..
if (tList[i1].CustomName.StartsWith("NAV:"))
{
Echo("Found NAV: command:");
tList[i1].CustomName = "NAV: C Arrived Target";
}
}
}
}
bWantFast = true;
doTriggerMain();
}
NavDebug(sNavDebug);
}
private static float GetIdealSliceSpacing(IVolumeHeader volumeHeader, Vector3D unitSpacingAxis)
{
// the ideal spacing is simply the diagonal of the voxel projected on to the spacing axis
// any larger than this value, and it becomes possible for an entire voxel to fit in between two consecutive output locations (i.e. missed for interpolation)
// any smaller than this value, and some voxels will have two or more output locations within their bounds
// note: voxel actually has 4 possible diagonals depending on the orientation, so we do this calculation for all diagonals and take the largest one
var p = volumeHeader.RotateToPatientOrientation(volumeHeader.VoxelSpacing);
return new[] {p, new Vector3D(-p.X, p.Y, p.Z), new Vector3D(p.X, -p.Y, p.Z), new Vector3D(-p.X, -p.Y, p.Z)}
.Select(v => Math.Abs(unitSpacingAxis.Dot(v))).Max();
}
double cos_ang(Vector3D n1, Vector3D n2)
{
return(System.Math.Max(-1.0, System.Math.Min(1.0, Vector3D.Dot(n1, n2))));
}
private void DoWork()
{
try
{
Vector3D velocity = Vector3D.Zero;
var grid = (Container.Entity as IMyCubeBlock).CubeGrid;
var entity = (Container.Entity as IMyFunctionalBlock);
MyFixedPoint amount = 0;
m_speed = 0;
if (grid != null && entity != null && grid.Physics != null)
{
velocity = grid.Physics.LinearVelocity;
var rotation = entity.WorldMatrix.GetDirectionVector(Base6Directions.Direction.Forward);
var start = entity.GetPosition() + (entity.WorldAABB.Size.Z / 2 * rotation);
var end = start + (100 * rotation);
if ((Container.Entity as IMyCubeBlock).CubeGrid.RayCastBlocks(start, end).HasValue)
{
m_state = RamscoopState.Blocked;
}
else if (!Vector3D.IsZero(velocity))
{
m_state = RamscoopState.Collecting;
m_speed = velocity.Length();
var rotdot = Vector3D.Dot(velocity, rotation);
var lens = velocity.Length() * rotation.Length();
var cos_theta = (rotdot / lens);
cos_theta += 1.0f; // positive offset, facing reverse will be 0.
m_cosTheta = cos_theta / 2.0d;
m_amountAdded = amount = m_amount * (MyFixedPoint)m_cosTheta * (MyFixedPoint)m_speed * (MyFixedPoint)m_sizeFactor;
}
}
if (Sync.IsServer && m_inventory.CanItemsBeAdded(amount, m_definitionId))
{
var content = (MyObjectBuilder_PhysicalObject)MyObjectBuilderSerializer.CreateNewObject(m_definitionId);
if (content != null)
{
MyAPIGateway.Utilities.InvokeOnGameThread(() =>
{
try
{ if (content != null)
{
m_inventory.AddItems(amount, content);
}
}
catch (Exception e)
{ Debug.HandleException(e); }
});
}
}
}
catch (Exception e)
{ Debug.HandleException(e); }
}
private static Matrix CalcRotateMatrixFromOrthogonalBasis(Vector3D xAxis, Vector3D yAxis, Vector3D zAxis)
{
const float zeroTolerance = 1e-4f;
Platform.CheckForNullReference(xAxis, "xAxis");
Platform.CheckForNullReference(yAxis, "yAxis");
Platform.CheckForNullReference(zAxis, "zAxis");
Platform.CheckFalse(xAxis.IsNull || yAxis.IsNull || zAxis.IsNull, "Input must be an orthogonal set of basis vectors (i.e. non-trivial vectors).");
Platform.CheckTrue(FloatComparer.AreEqual(xAxis.Dot(yAxis), 0, zeroTolerance)
&& FloatComparer.AreEqual(xAxis.Dot(zAxis), 0, zeroTolerance)
&& FloatComparer.AreEqual(yAxis.Dot(zAxis), 0, zeroTolerance), "Input must be an orthogonal set of basis vectors (i.e. mutually perpendicular).");
xAxis = xAxis.Normalize();
yAxis = yAxis.Normalize();
zAxis = zAxis.Normalize();
//TODO (CR Sept 2010): is this a rotation matrix, or the definition of a coordinate system?
var basis = new Matrix(4, 4);
basis.SetRow(0, xAxis.X, xAxis.Y, xAxis.Z, 0);
basis.SetRow(1, yAxis.X, yAxis.Y, yAxis.Z, 0);
basis.SetRow(2, zAxis.X, zAxis.Y, zAxis.Z, 0);
basis.SetRow(3, 0, 0, 0, 1);
return basis;
}
private static double ComputeAngle(ref Vector3D v1, ref Vector3D v2)
{
double cosa = v1.Dot(v2);
double angle = Math.Acos(MathUtil.ClampWithRange(cosa, -1, 1));
return angle; ;
}
public static Vector3D PoincareToKlein( Vector3D p )
{
double mag = 2 / (1 + p.Dot( p ));
return p * mag;
}
private GyroControl _Seek(ShipControlCommons shipControl,
Vector3D targetVector, Vector3D? targetUp,
out double yawError, out double pitchError,
out double rollError)
{
Vector3D referenceForward;
Vector3D referenceLeft;
Vector3D referenceUp;
GyroControl gyroControl;
// See if local orientation is the same as the ship
if (shipControl.ShipUp == ShipUp && shipControl.ShipForward == ShipForward)
{
// Use same reference vectors and GyroControl
referenceForward = shipControl.ReferenceForward;
referenceLeft = shipControl.ReferenceLeft;
referenceUp = shipControl.ReferenceUp;
gyroControl = shipControl.GyroControl;
}
else
{
referenceForward = GetReferenceVector(shipControl, ShipForward);
referenceLeft = GetReferenceVector(shipControl, ShipLeft);
referenceUp = GetReferenceVector(shipControl, ShipUp);
// Need our own GyroControl instance in this case
gyroControl = new GyroControl();
gyroControl.Init(shipControl.Blocks,
shipUp: ShipUp,
shipForward: ShipForward);
}
// Determine projection of targetVector onto our reference unit vectors
var dotZ = targetVector.Dot(referenceForward);
var dotX = targetVector.Dot(referenceLeft);
var dotY = targetVector.Dot(referenceUp);
var projZ = dotZ * referenceForward;
var projX = dotX * referenceLeft;
var projY = dotY * referenceUp;
// Determine yaw/pitch error by calculating angle between our forward
// vector and targetVector
var z = projZ.Length() * Math.Sign(dotZ);
var x = projX.Length() * Math.Sign(dotX);
var y = projY.Length() * Math.Sign(-dotY); // NB inverted
yawError = Math.Atan2(x, z);
pitchError = Math.Atan2(y, z);
var gyroYaw = yawPID.Compute(yawError);
var gyroPitch = pitchPID.Compute(pitchError);
gyroControl.SetAxisVelocityFraction(GyroControl.Yaw, (float)gyroYaw);
gyroControl.SetAxisVelocityFraction(GyroControl.Pitch, (float)gyroPitch);
if (targetUp != null)
{
// Also adjust roll
dotX = ((Vector3D)targetUp).Dot(referenceLeft);
dotY = ((Vector3D)targetUp).Dot(referenceUp);
projX = dotX * referenceLeft;
projY = dotY * referenceUp;
x = projX.Length() * Math.Sign(-dotX);
y = projY.Length() * Math.Sign(dotY);
rollError = Math.Atan2(x, y);
var gyroRoll = rollPID.Compute(rollError);
gyroControl.SetAxisVelocityFraction(GyroControl.Roll, (float)gyroRoll);
}
else
{
rollError = default(double);
}
return gyroControl;
}
public void RecalibrateCameraPosition(bool isCharacter = false)
{
// if (m_lookAt != Vector3D.Zero)
// return;
IMyCameraController cameraController = MySession.Static.CameraController;
if (cameraController == null || !(cameraController is MyEntity))
{
return;
}
Sandbox.Game.Entities.IMyControllableEntity controlledEntity = MySession.ControlledEntity as Sandbox.Game.Entities.IMyControllableEntity;
if (controlledEntity == null)
{
return;
}
// get latest head matrix
if (!isCharacter)
{
var headMatrix = controlledEntity.GetHeadMatrix(true);
m_targetOrientation = (Matrix)headMatrix.GetOrientation();
m_target = headMatrix.Translation;
}
// parent of hierarchy
MyEntity topControlledEntity = ((MyEntity)cameraController).GetTopMostParent();
if (topControlledEntity.Closed)
{
return;
}
// calculate controlled object coordinates in parent space
var worldToLocal = topControlledEntity.PositionComp.WorldMatrixNormalizedInv;
Vector3D targetInLocal = Vector3D.Transform(m_target, worldToLocal);
MatrixD orientationInLocal = m_targetOrientation * worldToLocal;
var localAABBHr = topControlledEntity.PositionComp.LocalAABBHr;
Vector3D centerToTarget = targetInLocal - localAABBHr.Center;
Vector3D backVec = Vector3D.Normalize(orientationInLocal.Backward);
// calculate offset for the
double projectedCenterToTarget = Vector3D.Dot(centerToTarget, backVec);
double projectedHalfExtent = Math.Abs(Vector3D.Dot(localAABBHr.HalfExtents, backVec));
double finalLength = projectedHalfExtent - projectedCenterToTarget;
Vector3D targetWithOffset = centerToTarget + (finalLength * backVec);
double width = LOOK_AT_DEFAULT_LENGTH; // some default value
if (Math.Abs(backVec.Z) > 0.0001)
{
width = localAABBHr.HalfExtents.X * 1.5f;
}
else if (Math.Abs(backVec.X) > 0.0001)
{
width = localAABBHr.HalfExtents.Z * 1.5f;
}
// calculate complete offset for controlled object
double halfFovTan = Math.Tan(MySector.MainCamera.FieldOfView * 0.5);
double offset = width / (2 * halfFovTan);
offset += finalLength;
double clampDist = MathHelper.Clamp(offset, GetMinDistance(), MAX_DISTANCE);
Vector3D lookAt = LOOK_AT_DIRECTION * clampDist;
SetPositionAndLookAt(lookAt);
}
public double MinAngle(Vector3D v0, Vector3D v1, Vector3D v2)
{
Vector3D a = (v1 - v0).normalized;
Vector3D b = (v2 - v1).normalized;
Vector3D c = (v0 - v2).normalized;
return(System.Math.Min(Vector3D.Dot(a, -c), System.Math.Min(Vector3D.Dot(b, -a), Vector3D.Dot(c, -b))));
}
/// <summary>
/// Checks two shapes for collisions.
/// </summary>
/// <param name="support1">The SupportMappable implementation of the first shape to test.</param>
/// <param name="support2">The SupportMappable implementation of the seconds shape to test.</param>
/// <param name="orientation1">The orientation of the first shape.</param>
/// <param name="orientation2">The orientation of the second shape.</param>
/// <param name="position1">The position of the first shape.</param>
/// <param name="position2">The position of the second shape</param>
/// <param name="point">The pointin world coordinates, where collision occur.</param>
/// <param name="normal">The normal pointing from body2 to body1.</param>
/// <param name="penetration">Estimated penetration depth of the collision.</param>
/// <returns>Returns true if there is a collision, false otherwise.</returns>
public static bool Detect(ISupportMappable2 support1, ISupportMappable2 support2, ref MatrixD orientation1,
ref MatrixD orientation2, ref Vector3D position1, ref Vector3D position2,
out Vector3D point, out Vector3D normal, out double penetration)
{
// Used variables
Vector3D temp1, temp2;
Vector3D v01, v02, v0;
Vector3D v11, v12, v1;
Vector3D v21, v22, v2;
Vector3D v31, v32, v3;
Vector3D v41, v42, v4;
Vector3D mn;
// Initialization of the output
point = normal = Vector3D.Zero;
penetration = 0.0f;
//Vector3 right = Vector3.Right;
// Get the center of shape1 in world coordinates -> v01
support1.SupportCenter(out v01);
Vector3D.Transform(ref v01, ref orientation1, out v01);
Vector3D.Add(ref position1, ref v01, out v01);
// Get the center of shape2 in world coordinates -> v02
support2.SupportCenter(out v02);
Vector3D.Transform(ref v02, ref orientation2, out v02);
Vector3D.Add(ref position2, ref v02, out v02);
// v0 is the center of the minkowski difference
Vector3D.Subtract(ref v02, ref v01, out v0);
// Avoid case where centers overlap -- any direction is fine in this case
if (v0.LengthSquared() < MathHelper.EPSILON * MathHelper.EPSILON)
{
v0 = new Vector3D(0.00001f, 0, 0);
}
// v1 = support in direction of origin
mn = v0;
Vector3D.Negate(ref v0, out normal);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v11);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v12);
Vector3D.Subtract(ref v12, ref v11, out v1);
if (Vector3D.Dot(v1, normal) <= 0.0f)
{
return(false);
}
// v2 = support perpendicular to v1,v0
Vector3D.Cross(ref v1, ref v0, out normal);
if (normal.LengthSquared() < MathHelper.EPSILON * MathHelper.EPSILON)
{
Vector3D.Subtract(ref v1, ref v0, out normal);
normal.Normalize();
point = v11;
Vector3D.Add(ref point, ref v12, out point);
Vector3D.Multiply(ref point, 0.5f, out point);
Vector3D.Subtract(ref v12, ref v11, out temp1);
penetration = Vector3D.Dot(temp1, normal);
//point = v11;
//point2 = v12;
return(true);
}
Vector3D.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v21);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v22);
Vector3D.Subtract(ref v22, ref v21, out v2);
if (Vector3D.Dot(v2, normal) <= 0.0f)
{
return(false);
}
// Determine whether origin is on + or - side of plane (v1,v0,v2)
Vector3D.Subtract(ref v1, ref v0, out temp1);
Vector3D.Subtract(ref v2, ref v0, out temp2);
Vector3D.Cross(ref temp1, ref temp2, out normal);
double dist = Vector3D.Dot(normal, v0);
// If the origin is on the - side of the plane, reverse the direction of the plane
if (dist > 0.0f)
{
Swap(ref v1, ref v2);
Swap(ref v11, ref v21);
Swap(ref v12, ref v22);
Vector3D.Negate(ref normal, out normal);
}
int phase2 = 0;
int phase1 = 0;
bool hit = false;
// Phase One: Identify a portal
while (true)
{
if (phase1 > MaximumIterations)
{
return(false);
}
phase1++;
// Obtain the support point in a direction perpendicular to the existing plane
// Note: This point is guaranteed to lie off the plane
Vector3D.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v31);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v32);
Vector3D.Subtract(ref v32, ref v31, out v3);
if (Vector3D.Dot(v3, normal) <= 0.0f)
{
return(false);
}
// If origin is outside (v1,v0,v3), then eliminate v2 and loop
Vector3D.Cross(ref v1, ref v3, out temp1);
if (Vector3D.Dot(temp1, v0) < 0.0f)
{
v2 = v3;
v21 = v31;
v22 = v32;
Vector3D.Subtract(ref v1, ref v0, out temp1);
Vector3D.Subtract(ref v3, ref v0, out temp2);
Vector3D.Cross(ref temp1, ref temp2, out normal);
continue;
}
// If origin is outside (v3,v0,v2), then eliminate v1 and loop
Vector3D.Cross(ref v3, ref v2, out temp1);
if (Vector3D.Dot(temp1, v0) < 0.0f)
{
v1 = v3;
v11 = v31;
v12 = v32;
Vector3D.Subtract(ref v3, ref v0, out temp1);
Vector3D.Subtract(ref v2, ref v0, out temp2);
Vector3D.Cross(ref temp1, ref temp2, out normal);
continue;
}
// Phase Two: Refine the portal
// We are now inside of a wedge...
while (true)
{
phase2++;
// Compute normal of the wedge face
Vector3D.Subtract(ref v2, ref v1, out temp1);
Vector3D.Subtract(ref v3, ref v1, out temp2);
Vector3D.Cross(ref temp1, ref temp2, out normal);
// Can this happen??? Can it be handled more cleanly?
if (normal.LengthSquared() < MathHelper.EPSILON * MathHelper.EPSILON)
{
return(true);
}
normal.Normalize();
// Compute distance from origin to wedge face
double d = Vector3D.Dot(normal, v1);
// If the origin is inside the wedge, we have a hit
if (d >= 0 && !hit)
{
// HIT!!!
hit = true;
}
// Find the support point in the direction of the wedge face
Vector3D.Negate(ref normal, out mn);
SupportMapTransformed(support1, ref orientation1, ref position1, ref mn, out v41);
SupportMapTransformed(support2, ref orientation2, ref position2, ref normal, out v42);
Vector3D.Subtract(ref v42, ref v41, out v4);
Vector3D.Subtract(ref v4, ref v3, out temp1);
double delta = Vector3D.Dot(temp1, normal);
penetration = Vector3D.Dot(v4, normal);
// If the boundary is thin enough or the origin is outside the support plane for the newly discovered vertex, then we can terminate
if (delta <= CollideEpsilon || penetration <= 0.0f || phase2 > MaximumIterations)
{
if (hit)
{
Vector3D.Cross(ref v1, ref v2, out temp1);
double b0 = Vector3D.Dot(temp1, v3);
Vector3D.Cross(ref v3, ref v2, out temp1);
double b1 = Vector3D.Dot(temp1, v0);
Vector3D.Cross(ref v0, ref v1, out temp1);
double b2 = Vector3D.Dot(temp1, v3);
Vector3D.Cross(ref v2, ref v1, out temp1);
double b3 = Vector3D.Dot(temp1, v0);
double sum = b0 + b1 + b2 + b3;
if (sum <= 0)
{
b0 = 0;
Vector3D.Cross(ref v2, ref v3, out temp1);
b1 = Vector3D.Dot(temp1, normal);
Vector3D.Cross(ref v3, ref v1, out temp1);
b2 = Vector3D.Dot(temp1, normal);
Vector3D.Cross(ref v1, ref v2, out temp1);
b3 = Vector3D.Dot(temp1, normal);
sum = b1 + b2 + b3;
}
double inv = 1.0f / sum;
Vector3D.Multiply(ref v01, b0, out point);
Vector3D.Multiply(ref v11, b1, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref v21, b2, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref v31, b3, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref v02, b0, out temp2);
Vector3D.Add(ref temp2, ref point, out point);
Vector3D.Multiply(ref v12, b1, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref v22, b2, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref v32, b3, out temp1);
Vector3D.Add(ref point, ref temp1, out point);
Vector3D.Multiply(ref point, inv * 0.5f, out point);
}
// Compute the barycentric coordinates of the origin
return(hit);
}
//// Compute the tetrahedron dividing face (v4,v0,v1)
//Vector3.Cross(ref v4, ref v1, out temp1);
//double d1 = Vector3.Dot(ref temp1, ref v0);
//// Compute the tetrahedron dividing face (v4,v0,v2)
//Vector3.Cross(ref v4, ref v2, out temp1);
//double d2 = Vector3.Dot(ref temp1, ref v0);
// Compute the tetrahedron dividing face (v4,v0,v3)
Vector3D.Cross(ref v4, ref v0, out temp1);
double dot = Vector3D.Dot(temp1, v1);
if (dot >= 0.0f)
{
dot = Vector3D.Dot(temp1, v2);
if (dot >= 0.0f)
{
// Inside d1 & inside d2 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
else
{
// Inside d1 & outside d2 ==> eliminate v3
v3 = v4;
v31 = v41;
v32 = v42;
}
}
else
{
dot = Vector3D.Dot(temp1, v3);
if (dot >= 0.0f)
{
// Outside d1 & inside d3 ==> eliminate v2
v2 = v4;
v21 = v41;
v22 = v42;
}
else
{
// Outside d1 & outside d3 ==> eliminate v1
v1 = v4;
v11 = v41;
v12 = v42;
}
}
}
}
}
public double GetSquared()
{
if (DistanceSquared >= 0)
{
return(DistanceSquared);
}
Vector3D diff = triangle.V0 - point;
Vector3D edge0 = triangle.V1 - triangle.V0;
Vector3D edge1 = triangle.V2 - triangle.V0;
double a00 = edge0.LengthSquared;
double a01 = edge0.Dot(edge1);
double a11 = edge1.LengthSquared;
double b0 = diff.Dot(edge0);
double b1 = diff.Dot(edge1);
double c = diff.LengthSquared;
double det = Math.Abs(a00 * a11 - a01 * a01);
double s = a01 * b1 - a11 * b0;
double t = a01 * b0 - a00 * b1;
double sqrDistance;
if (s + t <= det)
{
if (s < 0)
{
if (t < 0) // region 4
{
if (b0 < 0)
{
t = 0;
if (-b0 >= a00)
{
s = 1;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = -b0 / a00;
sqrDistance = b0 * s + c;
}
}
else
{
s = 0;
if (b1 >= 0)
{
t = 0;
sqrDistance = c;
}
else if (-b1 >= a11)
{
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
t = -b1 / a11;
sqrDistance = b1 * t + c;
}
}
}
else // region 3
{
s = 0;
if (b1 >= 0)
{
t = 0;
sqrDistance = c;
}
else if (-b1 >= a11)
{
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
t = -b1 / a11;
sqrDistance = b1 * t + c;
}
}
}
else if (t < 0) // region 5
{
t = 0;
if (b0 >= 0)
{
s = 0;
sqrDistance = c;
}
else if (-b0 >= a00)
{
s = 1;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = -b0 / a00;
sqrDistance = b0 * s + c;
}
}
else // region 0
{
// minimum at interior point
double invDet = (1) / det;
s *= invDet;
t *= invDet;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
else
{
double tmp0, tmp1, numer, denom;
if (s < 0) // region 2
{
tmp0 = a01 + b0;
tmp1 = a11 + b1;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a00 - (2) * a01 + a11;
if (numer >= denom)
{
s = 1;
t = 0;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = numer / denom;
t = 1 - s;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
else
{
s = 0;
if (tmp1 <= 0)
{
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else if (b1 >= 0)
{
t = 0;
sqrDistance = c;
}
else
{
t = -b1 / a11;
sqrDistance = b1 * t + c;
}
}
}
else if (t < 0) // region 6
{
tmp0 = a01 + b1;
tmp1 = a00 + b0;
if (tmp1 > tmp0)
{
numer = tmp1 - tmp0;
denom = a00 - (2) * a01 + a11;
if (numer >= denom)
{
t = 1;
s = 0;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
t = numer / denom;
s = 1 - t;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
else
{
t = 0;
if (tmp1 <= 0)
{
s = 1;
sqrDistance = a00 + (2) * b0 + c;
}
else if (b0 >= 0)
{
s = 0;
sqrDistance = c;
}
else
{
s = -b0 / a00;
sqrDistance = b0 * s + c;
}
}
}
else // region 1
{
numer = a11 + b1 - a01 - b0;
if (numer <= 0)
{
s = 0;
t = 1;
sqrDistance = a11 + (2) * b1 + c;
}
else
{
denom = a00 - (2) * a01 + a11;
if (numer >= denom)
{
s = 1;
t = 0;
sqrDistance = a00 + (2) * b0 + c;
}
else
{
s = numer / denom;
t = 1 - s;
sqrDistance = s * (a00 * s + a01 * t + (2) * b0) +
t * (a01 * s + a11 * t + (2) * b1) + c;
}
}
}
}
// Account for numerical round-off error.
if (sqrDistance < 0)
{
sqrDistance = 0;
}
DistanceSquared = sqrDistance;
TriangleClosest = triangle.V0 + s * edge0 + t * edge1;
TriangleBaryCoords = new Vector3D(1 - s - t, s, t);
return(sqrDistance);
}
/**
* Computes the distance between two 3D segments.
*
* @param A the start point of the first segment
* @param B the end point of the first segment
* @param C the start point of the second segment
* @param D the end point of the second segment
* @return the distance between the segments
*/
public static double DistanceSegmentSegment(
Coordinate A, Coordinate B, Coordinate C, Coordinate D)
{
/**
* This calculation is susceptible to roundoff errors when
* passed large ordinate values.
* It may be possible to improve this by using {@link DD} arithmetic.
*/
if (A.Equals3D(B))
{
return(DistancePointSegment(A, C, D));
}
if (C.Equals3D(B))
{
return(DistancePointSegment(C, A, B));
}
/**
* Algorithm derived from http://softsurfer.com/Archive/algorithm_0106/algorithm_0106.htm
*/
var a = Vector3D.Dot(A, B, A, B);
var b = Vector3D.Dot(A, B, C, D);
var c = Vector3D.Dot(C, D, C, D);
var d = Vector3D.Dot(A, B, C, A);
var e = Vector3D.Dot(C, D, C, A);
var denom = a * c - b * b;
if (Double.IsNaN(denom))
{
throw new ArgumentException("Ordinates must not be NaN");
}
double s;
double t;
if (denom <= 0.0)
{
/**
* The lines are parallel.
* In this case solve for the parameters s and t by assuming s is 0.
*/
s = 0;
// choose largest denominator for optimal numeric conditioning
if (b > c)
{
t = d / b;
}
else
{
t = e / c;
}
}
else
{
s = (b * e - c * d) / denom;
t = (a * e - b * d) / denom;
}
if (s < 0)
{
return(DistancePointSegment(A, C, D));
}
if (s > 1)
{
return(DistancePointSegment(B, C, D));
}
if (t < 0)
{
return(DistancePointSegment(C, A, B));
}
if (t > 1)
{
return(DistancePointSegment(D, A, B));
}
/**
* The closest points are in interiors of segments,
* so compute them directly
*/
var x1 = A.X + s * (B.X - A.X);
var y1 = A.Y + s * (B.Y - A.Y);
var z1 = A.Z + s * (B.Z - A.Z);
var x2 = C.X + t * (D.X - C.X);
var y2 = C.Y + t * (D.Y - C.Y);
var z2 = C.Z + t * (D.Z - C.Z);
// length (p1-p2)
return(Distance(new Coordinate(x1, y1, z1), new Coordinate(x2, y2, z2)));
}
/// <summary>
/// Calculate the cosine of the angle between two vectors.
/// </summary>
private static double CosAngle( Vector3D p1, Vector3D p2 )
{
double cosA = p1.Dot( p2 ) / (p1.Abs() * p2.Abs());
return Clamp( cosA );
}
public void Dot()
{
Vector3D a = new Vector3D(1.0, 2.0, 3.0);
Vector3D b = new Vector3D(4.0, 5.0, 6.0);
double dot = a.Dot(b);
Assert.AreEqual(1.0 * 4.0 + 2.0 * 5.0 + 3.0 * 6.0, dot, 1e-14);
}
