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); }