public static URVector Add(URVector vector1, URVector vector2)
{
var newPoseVector = Vector3D.Add(vector1.PoseVector, vector2.PoseVector);
var newRotationVector = Vector3D.Add(vector1.RotationVector, vector2.RotationVector);
return(new URVector(newPoseVector, newRotationVector));
}
Vector3D ApplyTarSpd(Vector3D position, Vector3D speed, Vector3D myPosition, Vector3D myVel)
{
double
mySpeed = myVel.Length(),
enSpeed = speed.Length(),
multiplier;
//position = Vector3D.Add(position, Vector3D.Multiply(speed,4 / 60));
if (enSpeed > 0)
{
Vector3D output = GetProjectedPos(position, speed, myPosition, myVel);
if (!output.Equals(NOTHING))
{
return(output);
}
}
multiplier = (mySpeed != 0 && enSpeed != 0) ? (enSpeed / mySpeed) : 0;
Vector3D
addition = Vector3D.Multiply(speed, multiplier);
return(Vector3D.Add(position, addition));
}
public Vector3D LinearIntarp(double t)
{
Vector3D ret = p0.SclProd(1.0D - t);
Vector3D temp = p1.SclProd(t);
return(ret.Add(temp));
}
private void SumButton(object sender, RoutedEventArgs e)
{
firstVecFromUser = goThroughText(FirstVec3X.Text, FirstVec3Y.Text, FirstVec3Z.Text);
secondVecFromUser = goThroughText(SecondVec3X.Text, SecondVec3Y.Text, SecondVec3Z.Text);
Vector3D.Add(firstVecFromUser, secondVecFromUser);
}
private async Task Draw(SensorReadings readings)
{
Point3D point1 = new Point3D(-readings.Reading0, _position, 2);
Point3D point2 = new Point3D(-(readings.Reading45 * Math.Cos(Math.PI / 2)), _position, (readings.Reading45 * Math.Sin(Math.PI / 2)) + 2);
Point3D point3 = new Point3D(0, _position, readings.Reading90 + 2);
Point3D point4 = new Point3D(readings.Reading135 * Math.Cos(Math.PI / 2), _position, (readings.Reading135 * Math.Sin(Math.PI / 2)) + 2);
Point3D point5 = new Point3D(readings.Reading180, _position, 2);
Debug.WriteLine($"{{{point1}}}, {{{point2}}}, {{{point3}}}, {{{point4}}}, {{{point5}}}");
MapPlotter.Points.Add(point1);
MapPlotter.Points.Add(point2);
MapPlotter.Points.Add(point2);
MapPlotter.Points.Add(point3);
MapPlotter.Points.Add(point3);
MapPlotter.Points.Add(point4);
MapPlotter.Points.Add(point4);
MapPlotter.Points.Add(point5);
_position++;
Viewport.Camera.LookDirection = Vector3D.Add(Viewport.Camera.LookDirection, new Vector3D(0, 1, 0));
await Task.Delay(200);
}
public static URPose Add(URVector vector, URPose pose)
{
var newPosition = Vector3D.Add(vector.PoseVector, pose.Position);
var newRotation = Vector3D.Add(vector.RotationVector, pose.Rotation);
return(new URPose(newPosition, newRotation));
}
/// <summary>
/// Sphere test for simple entities, like floating objects, and for avoiding dangerous ships tools.
/// </summary>
/// <param name="entity">The entity to test.</param>
/// <param name="input"><see cref="TestInput"/></param>
/// <param name="result"><see cref="GridTestResult"/></param>
/// <returns>True if the entity is obstructing the ship.</returns>
private bool SphereTest(MyEntity entity, ref TestInput input, ref GridTestResult result)
{
Vector3D currentPosition = AutopilotGrid.GetCentre();
Vector3D rejectD = input.Direction;
Vector3D disp; Vector3D.Multiply(ref rejectD, input.Length, out disp);
Vector3D finalPosition; Vector3D.Add(ref currentPosition, ref disp, out finalPosition);
float shipRadius = AutopilotGrid.PositionComp.LocalVolume.Radius;
Vector3D obsPos = entity.GetCentre();
Vector3D offObsPos; Vector3D.Subtract(ref obsPos, ref input.Offset, out offObsPos);
float obsRadius = entity.PositionComp.LocalVolume.Radius;
if (entity is MyCubeBlock)
{
// it is an active tool so increase the radius
obsRadius += 10f;
}
m_lineSegment.From = currentPosition;
m_lineSegment.To = finalPosition;
Vector3D closest;
result.Distance = (float)m_lineSegment.ClosestPoint(ref offObsPos, out closest);
double distance; Vector3D.Distance(ref offObsPos, ref closest, out distance);
result.Proximity = (float)distance - (shipRadius + obsRadius);
if (result.Proximity <= 0f && IsRejectionTowards(ref obsPos, ref currentPosition, ref rejectD))
{
Logger.DebugLog("Rejection " + input.Direction + " hit " + entity.nameWithId());
result.ObstructingBlock = entity as MyCubeBlock; // it may not be a block
return(true);
}
return(false);
}
public void PointFromTValue(double tValue, out Vector3D point)
{
Vector3D direction = Direction;
Vector3D disp; Vector3D.Multiply(ref direction, tValue, out disp);
Vector3D.Add(ref m_line.From, ref disp, out point);
}
public void TestBasicOperatorsMethod()
{
Vector3D vectorA = new Vector3D(1, 2, 3);
Vector3D vectorB = new Vector3D(1, 4, 6);
Vector3D vectorAdd = new Vector3D(2, 6, 9);
Assert.IsTrue(vectorAdd == Vector3D.Add(vectorA, vectorB), String.Format("Expected for '{0}': true; Actual: {1}", vectorAdd, true));
Vector3D substrationResult = new Vector3D(0, -2, -3);
Assert.IsTrue(substrationResult == Vector3D.Substration(vectorA, vectorB), String.Format("Expected for '{0}': true; Actual: {1}", substrationResult, true));
Vector3D negationResult = new Vector3D(-1, -2, -3);
Assert.IsTrue(negationResult == Vector3D.Negation(vectorA));
Vector3D multiByScalarResult = new Vector3D(2, 4, 6);
Assert.IsTrue(multiByScalarResult == vectorA * (2));
Vector3D dividedByScalarResult = new Vector3D(1, 2, 3);
Assert.IsTrue(dividedByScalarResult == (vectorA * (3)) / (3));
}
private void OnMouseMove(object sender, MouseEventArgs e)
{
Vector delta = (e.GetPosition(Viewport) - oldMousePos);
BlocksViewModel vm = (BlocksViewModel)((FrameworkElement)sender).DataContext; // TODO: Ugly dependency on the model here!
ProjectionCamera camera = (ProjectionCamera)Viewport.Camera;
Vector3D left = Vector3D.CrossProduct(camera.LookDirection, camera.UpDirection);
left.Normalize();
switch (cameraDragMode)
{
case CameraDragMode.Pan:
delta *= 0.01;
Vector3D deltaMove = Vector3D.Add(left * -delta.X, camera.UpDirection * delta.Y);
Point3D newCameraPosition = vm.CameraPosition.Add(deltaMove);
vm.CameraPosition = newCameraPosition;
Point3D newCameraAimPoint = vm.CameraAimPoint.Add(deltaMove);
vm.CameraAimPoint = newCameraAimPoint;
break;
case CameraDragMode.Rotate:
delta *= 0.01;
Vector3D v = vm.CameraAimPoint.VectorTo(vm.CameraPosition).Cartesian2Spherical();
v.Y -= delta.X;
v.Z -= delta.Y;
vm.CameraPosition = vm.CameraAimPoint.Add(v.Spherical2Cartesian());
break;
}
oldMousePos = e.GetPosition(Viewport);
}
public void ProcessDockingRequest()
{
Drone.LogToLcd($"\nLogging: {DateTime.Now}");
MyIGCMessage message = this.Drone.NetworkService.GetBroadcastListenerForChannel(DockingRequestChannel).AcceptMessage();
if (message.Data == null)
{
Drone.LogToLcd($"\nNo Message");
}
IMyShipConnector dockingPort = Drone.Grid().GetBlockWithName("Docking Port 1") as IMyShipConnector;
if (dockingPort == null)
{
Drone.LogToLcd("\nDocking Port 1 not found.");
}
else
{
Vector3D approachPoint = Vector3D.Add(dockingPort.GetPosition(), Vector3D.Multiply(dockingPort.WorldMatrix.Forward, 50));
List <Vector3D> dockingPath = new List <Vector3D> {
approachPoint, dockingPort.GetPosition()
};
Drone.LogToLcd($"Sending message: {dockingPort.WorldMatrix.Forward},{dockingPath[0].ToString()},{dockingPath[1].ToString()}");
this.Drone.NetworkService.BroadcastMessage(
DockingRequestChannel,
$"{dockingPort.WorldMatrix.Forward},{dockingPath[0].ToString()},{dockingPath[1].ToString()}"
);
}
}
internal void Calc_dynamics(int lr, AirPlane ap, Vector3D dv)
{
fv.SetVec(0.0D, 0.0D, 0.0D);
tv.SetVec(0.0D, 0.0D, 0.0D);
if (flag == 0)
{
return;
}
ac = AirPlane.Get_point(ac_base, lr).Sub(ap.inp.cg);
vd = Dynamics.VWithRot(ap.pMotion.vc, ap.pMotion.omega, ac);
vd = vd.Add(dv);
v = vd.Length();
q = (0.5D * v * v * ap.atmos.rho);
Bearing3 br = new Bearing3(ap.pMotion.vc.R2l());
angle = br.pitch.GetValue();
sfus = (s_pi * Math.Cos(angle) + s_side * Math.Sin(angle));
d = (q * cd_s / s_pi * sfus);
du = vd.SclProd(-1.0D).NmlVec();
fv = du.SclProd(d);
mfus = (q * vfus * Math.Sin(-2.0D * angle));
tv.SetVec(0.0D, mfus, 0.0D);
Matrix44 mat = new Matrix44();
mat.SetRxMat(-br.roll.GetValue());
tv = tv.MultMat(mat);
}
//zoom
private void MainWindow_PreviewMouseWheel(object sender, MouseWheelEventArgs e)
{
double factor = UnitConversion.Convert(0.1, Length_Unit_Types.FEET, _host.BIM_To_OSM.UnitType);
var direction = this.camera.LookDirection;
direction.Normalize();
this.camera.Position = Vector3D.Add(direction * factor * e.Delta, this.camera.Position);
}
public void Move(Vector3D direction)
{
_position = _position.Add(direction);
_origv1 = _origv1.Add(direction);
_origv2 = _origv2.Add(direction);
_origv3 = _origv3.Add(direction);
Transform();
}
public void Addition()
{
Vector3D a = new Vector3D(1.0, 2.0, 3.0);
Vector3D b = new Vector3D(2.0, 3.0, 4.0);
Vector3D c = Vector3D.Add(a, b);
Assert.AreEqual(new Vector3D(3.0, 5.0, 7.0), c);
}
private void BlockTest(AeroCell currentData, ref Vector3I currentCell, ref Vector3 targetCellAvg)
{
float gridSize = m_grid.GridSize;
double minLineLength = gridSize * 1.4d;
int tries;
for (tries = 0; tries < 100; ++tries)
{
if (currentData.CurVelocity.AbsMax() < 0.1f)
{
Log.TraceLog("Air is stopped at " + currentCell + ", " + currentData);
break;
}
// check for blocks between here and target
Vector3 targetLocalAvg; Vector3.Multiply(ref targetCellAvg, gridSize, out targetLocalAvg);
LineD line = new LineD(currentCell * (double)gridSize, targetLocalAvg);
if (line.Length < minLineLength)
{
line.Length = minLineLength;
Vector3D disp; Vector3D.Multiply(ref line.Direction, minLineLength, out disp);
Vector3D.Add(ref line.From, ref disp, out line.To);
}
MyIntersectionResultLineTriangleEx?result;
if (m_grid.Intersects(ref line, m_rayCastCells, out result, IntersectionFlags.ALL_TRIANGLES))
{
Vector3 normal = result.Value.NormalInObjectSpace;
if (result.Value.Entity != m_grid)
{
Matrix localMatrix = result.Value.Entity.LocalMatrix.GetOrientation();
Vector3.Transform(ref normal, ref localMatrix, out normal);
}
BlockTest_ApplyNormal(currentData, ref currentCell, ref normal, ref targetCellAvg);
continue;
}
else
{
Vector3I bestTarget;
bestTarget.X = (int)Math.Round(targetCellAvg.X);
bestTarget.Y = (int)Math.Round(targetCellAvg.Y);
bestTarget.Z = (int)Math.Round(targetCellAvg.Z);
if (m_grid.CubeExists(bestTarget))
{
Vector3 hitNormal;
if (BlockTest_Multi(ref line, out hitNormal))
{
BlockTest_ApplyNormal(currentData, ref currentCell, ref hitNormal, ref targetCellAvg);
continue;
}
}
break;
}
}
Log.DebugLog("Too many tries", Logger.severity.WARNING, condition: tries >= 100);
currentData.BlockTest = true;
}
/// <summary>
/// Generates a random amount of moons for a planet
/// </summary>
/// <param name="parentPlanet"></param>
/// <returns></returns>
private MySystemPlanetMoon[] GeneratePlanetMoons(MySystemPlanet parentPlanet)
{
MyPluginLog.Log("Generating moons for planet " + parentPlanet.DisplayName);
var settings = MySettingsSession.Static.Settings.GeneratorSettings;
uint maxMoons = (uint)Math.Ceiling(parentPlanet.Diameter / 120000 * 2);
maxMoons = (uint)Math.Min(maxMoons, settings.PlanetSettings.MinMaxMoons.Max);
uint numMoons = (uint)MyRandom.Instance.Next(settings.PlanetSettings.MinMaxMoons.Min, (int)maxMoons + 1);
numMoons = Math.Min(numMoons, 25);
if (settings.SystemGenerator == SystemGenerationMethod.UNIQUE)
{
numMoons = Math.Min(numMoons, (uint)m_moons.Count);
}
MySystemPlanetMoon[] moons = new MySystemPlanetMoon[numMoons];
for (int i = 0; i < numMoons; i++)
{
MyPluginLog.Log("Generating moon " + i);
double distance = parentPlanet.Diameter * (i + 1) + parentPlanet.Diameter * MyRandom.Instance.GetRandomFloat(1f, 1.5f);
double diameter;
var definition = FindPlanetDefinitionForSize(parentPlanet.Diameter * 0.75f, out diameter, true);
if (definition == null)
{
return(moons);
}
Vector3D position;
int tries2 = 0;
do
{
double angle = MyRandom.Instance.GetRandomFloat(0, (float)Math.PI * 2f);
position = new Vector3D(distance * Math.Sin(angle), distance * Math.Cos(angle), distance * Math.Sin(MyRandom.Instance.GetRandomFloat((float)-Math.PI / 2, (float)Math.PI / 2)));
position = Vector3D.Add(position, parentPlanet.CenterPosition);
} while (IsMoonPositionObstructed(position, diameter, parentPlanet, moons) && ++tries2 < MAX_DEF_FIND_ROUNDS);
MySystemPlanetMoon moon = new MySystemPlanetMoon();
moon.CenterPosition = position;
moon.Diameter = diameter;
moon.DisplayName = GetMoonName(i, definition.Id.SubtypeId.String, parentPlanet.DisplayName);
moon.SubtypeId = definition.Id.SubtypeId.String;
moon.ParentId = parentPlanet.Id;
moons[i] = moon;
}
MyPluginLog.Log("Generating moons for planet " + parentPlanet.DisplayName + " completed");
return(moons);
}
/// <summary>
/// Estimates the proximity between a given capsule and a voxel.
/// </summary>
/// <param name="capsule">The capsule to test for intersection, the points must be at least one metre apart.</param>
public static double Proximity(ref CapsuleD capsule, MyVoxelBase voxel, ref Vector3D hitPosition, double capsuleLength = -1d)
{
Logger.TraceLog(capsule.String());
if (capsuleLength < 0)
{
Vector3D.Distance(ref capsule.P0, ref capsule.P1, out capsuleLength);
}
double halfLength = capsuleLength * 0.5d;
Vector3D temp; Vector3D.Add(ref capsule.P0, ref capsule.P1, out temp);
Vector3D middle; Vector3D.Multiply(ref temp, 0.5d, out middle);
if (capsuleLength < 1d)
{
hitPosition = middle;
Logger.TraceLog("capsule length < 1: " + capsuleLength);
return(capsuleLength);
}
double radius = halfLength + capsule.Radius;
BoundingSphereD worldSphere = new BoundingSphereD()
{
Center = middle, Radius = radius
};
if (capsuleLength < Math.Max(capsule.Radius, 1f) * 8f)
{
if (!voxel.ContainsOrIntersects(ref worldSphere))
{
Logger.TraceLog("sphere does not intersect voxel: " + worldSphere + ", capsuleLength: " + capsuleLength);
return(capsuleLength);
}
}
else if (!voxel.PositionComp.WorldAABB.Intersects(ref worldSphere))
{
Logger.TraceLog("sphere does not intersect voxel AABB: " + worldSphere + ", capsuleLength: " + capsuleLength);
return(capsuleLength);
}
CapsuleD halfCapsule;
halfCapsule.P0 = capsule.P0;
halfCapsule.P1 = middle;
halfCapsule.Radius = capsule.Radius;
double prox = Proximity(ref halfCapsule, voxel, ref hitPosition, halfLength);
if (prox < 1f)
{
return(prox);
}
halfCapsule.P0 = middle;
halfCapsule.P1 = capsule.P1;
return(Math.Min(prox, Proximity(ref halfCapsule, voxel, ref hitPosition, halfLength)));
}
public void AddDoubleExample()
{
var vector = new Vector3D(4, 7, 3);
vector.Add(1);
Assert.AreEqual(5, vector.X);
Assert.AreEqual(8, vector.Y);
Assert.AreEqual(4, vector.Z);
}
/// <summary>
/// Create an OBB from a list of 8 verticies and align it to a grid
/// </summary>
/// <param name="grid"></param>
/// <param name="verticies"></param>
/// <param name="offset">Allows you to expand or shrink the box by a given number of meters</param>
/// <returns></returns>
public static MyOrientedBoundingBoxD CreateOrientedBoundingBox(IMyCubeGrid grid, List <Vector3D> verticies, double offset = 0)
{
//create the quaternion to rotate the box around
Quaternion yardQuaternion = Quaternion.CreateFromForwardUp(
grid.WorldMatrix.Forward,
grid.WorldMatrix.Up);
//find the center of the volume
var yardCenter = new Vector3D();
foreach (Vector3D vertex in verticies)
{
yardCenter = Vector3D.Add(yardCenter, vertex);
}
yardCenter = Vector3D.Divide(yardCenter, verticies.Count);
//find the dimensions of the box.
//convert verticies to grid coordinates to find adjoining neighbors
var gridVerticies = new List <Vector3I>(verticies.Count);
foreach (Vector3D vertext in verticies)
{
gridVerticies.Add(grid.WorldToGridInteger(vertext));
}
double xLength = 0d;
double yLength = 0d;
double zLength = 0d;
//finds the length of each axis
for (int i = 1; i < gridVerticies.Count; ++i)
{
if (gridVerticies[0].Y == gridVerticies[i].Y && gridVerticies[0].Z == gridVerticies[i].Z)
{
xLength = Math.Abs(gridVerticies[0].X - gridVerticies[i].X) * grid.GridSize;
continue;
}
if (gridVerticies[0].X == gridVerticies[i].X && gridVerticies[0].Z == gridVerticies[i].Z)
{
yLength = Math.Abs(gridVerticies[0].Y - gridVerticies[i].Y) * grid.GridSize;
continue;
}
if (gridVerticies[0].X == gridVerticies[i].X && gridVerticies[0].Y == gridVerticies[i].Y)
{
zLength = Math.Abs(gridVerticies[0].Z - gridVerticies[i].Z) * grid.GridSize;
}
}
var halfExtents = new Vector3D(offset + xLength / 2, offset + yLength / 2, offset + zLength / 2);
//FINALLY we can make the bounding box
return(new MyOrientedBoundingBoxD(yardCenter, halfExtents, yardQuaternion));
}
public void AddSurface(Surface newSurface)
{
Surfaces.Add(newSurface);
if (newSurface.IsExterior)
{
ExteriorSurfaces.Add(newSurface);
}
DragCoefficients.Add(newSurface.DragCoefficient);
}
public void adding_vector_adds_to_components()
{
var actual = new Vector3D(1, 2, 10);
var right = new Vector3D(3, 4, 11);
var expected = new Vector3D(4, 6, 21);
actual.Add(right);
Assert.Equal(expected, actual);
}
public bool CanTarget(Vector3D target)
{
Vector3D fwd = Vector3D.Add(this.CTRL.GetPosition(), Vector3D.Multiply(this.BSDCon.WorldMatrix.Forward, 1000d));
if (InterCosine(fwd, target) >= Math.Cos(maxAngle * (Math.PI / 180.0)))
{
return(true);
}
return(false);
}
public void adding_vectors_leaves_right_unchanged()
{
var left = new Vector3D(1, 2, 10);
var right = new Vector3D(3, 4, 11);
var expectedRight = new Vector3D(right);
left.Add(right);
Assert.Equal(expectedRight, right);
}
public void TestAdd_2()
{
var v1 = new Vector3D(1d, 2d, 3d);
v1.Add(29d);
Assert.AreEqual(30d, v1.X);
Assert.AreEqual(31d, v1.Y);
Assert.AreEqual(32d, v1.Z);
}
public void Update()
{
cg.SetVec(0.0D, 0.0D, 0.0D);
m = 0.0D;
int i;
for (i = 0; i < MAX_BLOCK; i++)
{
InertiaBlock ib;
if ((ib = block[i]) != null)
{
m += ib.m;
cg = cg.Add(ib.M_cg());
}
}
if (m > 0.0D)
{
cg = cg.SclProd(1.0D / m);
}
ixx = (iyy = izz = ixy = iyz = izx = 0.0D);
for (i = 0; i < MAX_BLOCK; i++)
{
InertiaBlock ib_0;
if ((ib_0 = block[i]) != null)
{
ixx += ib_0.Ixx(cg);
iyy += ib_0.Iyy(cg);
izz += ib_0.Izz(cg);
ixy += ib_0.Ixy(cg);
iyz += ib_0.Iyz(cg);
izx += ib_0.Izx(cg);
}
}
InertiaMat.SetUMat();
InertiaMat.element[0, 0] = ixx;
InertiaMat.element[1, 1] = iyy;
InertiaMat.element[2, 2] = izz;
double tmp330_329 = (-ixy);
InertiaMat.element[1, 0] = tmp330_329;
InertiaMat.element[0, 1] = tmp330_329;
double tmp358_357 = (-izx);
InertiaMat.element[2, 0] = tmp358_357;
InertiaMat.element[0, 2] = tmp358_357;
double tmp386_385 = (-iyz);
InertiaMat.element[2, 1] = tmp386_385;
InertiaMat.element[1, 2] = tmp386_385;
InertiaInvMat = InertiaMat.InvMat();
}
/// <summary>
/// Populates outHitPositions with cell positions of the grid, regardless of whether or not there are blocks occupying those cells.
/// </summary>
/// <param name="grid">The grid to get cell positions of.</param>
/// <param name="localStart">The local position to start from.</param>
/// <param name="localEnd">The local position to end at.</param>
/// <param name="outHitPositions">Populated with cells that the line passes through.</param>
public static void RayCastCellsLocal(this IMyCubeGrid grid, ref Vector3D localStart, ref Vector3D localEnd, List <Vector3I> outHitPositions)
{
Vector3D offset = ((MyCubeGrid)grid).GridSizeHalfVector;
Vector3D offStart; Vector3D.Add(ref localStart, ref offset, out offStart);
Vector3D offEnd; Vector3D.Add(ref localEnd, ref offset, out offEnd);
Vector3I min = grid.Min - Vector3I.One;
Vector3I max = grid.Max + Vector3I.One;
MyGridIntersection.Calculate(outHitPositions, grid.GridSize, offStart, offEnd, min, max);
}
public void Vector3AddTest()
{
Vector3D a = new Vector3D(1.0f, 2.0f, 3.0f);
Vector3D b = new Vector3D(5.0f, 6.0f, 7.0f);
Vector3D expected = new Vector3D(6.0f, 8.0f, 10.0f);
Vector3D actual;
actual = Vector3D.Add(a, b);
Assert.AreEqual(expected, actual);
}
/// <summary>
/// Performs a binary search for intersection using spheres.
/// </summary>
/// <param name="hitPosition">a point on the capsule's line close to obstruction</param>
public static bool Intersects(ref CapsuleD capsule, MyVoxelBase voxel, out Vector3D hitPosition, double capsuleLength = -1d)
{
//Logger.DebugLog("P0: " + capsule.P0 + ", P1: " + capsule.P1 + ", radius: " + capsule.Radius);
if (capsuleLength < 0)
{
Vector3D.Distance(ref capsule.P0, ref capsule.P1, out capsuleLength);
}
double halfLength = capsuleLength * 0.5d;
Vector3D temp; Vector3D.Add(ref capsule.P0, ref capsule.P1, out temp);
Vector3D middle; Vector3D.Multiply(ref temp, 0.5d, out middle);
if (capsuleLength < 1f)
{
hitPosition = middle;
return(true);
}
double radius = halfLength + capsule.Radius;
BoundingSphereD worldSphere = new BoundingSphereD()
{
Center = middle, Radius = radius
};
if (capsuleLength < Math.Max(capsule.Radius, 1f) * 8f)
{
if (!voxel.ContainsOrIntersects(ref worldSphere))
{
hitPosition = Globals.Invalid;
return(false);
}
}
else if (!voxel.PositionComp.WorldAABB.Intersects(ref worldSphere))
{
hitPosition = Globals.Invalid;
return(false);
}
CapsuleD halfCapsule;
halfCapsule.P0 = capsule.P0;
halfCapsule.P1 = middle;
halfCapsule.Radius = capsule.Radius;
if (Intersects(ref halfCapsule, voxel, out hitPosition, halfLength))
{
return(true);
}
halfCapsule.P0 = middle;
halfCapsule.P1 = capsule.P1;
return(Intersects(ref halfCapsule, voxel, out hitPosition, halfLength));
}
public void Vector3AddTest()
{
Vector3D <float> a = new Vector3D <float>(1.0f, 2.0f, 3.0f);
Vector3D <float> b = new Vector3D <float>(5.0f, 6.0f, 7.0f);
Vector3D <float> expected = new Vector3D <float>(6.0f, 8.0f, 10.0f);
Vector3D <float> actual;
actual = Vector3D.Add(a, b);
Assert.Equal(expected, actual);
}
public void Add()
{
Vector3D v1 = new Vector3D(1.0, 2.0, 3.0);
Vector3D v2 = new Vector3D(4.0, 5.0, 6.0);
Vector3D v3 = v1 + v2;
Assert.AreEqual(5.0, v3.X, 1e-14);
Assert.AreEqual(7.0, v3.Y, 1e-14);
Assert.AreEqual(9.0, v3.Z, 1e-14);
Vector3D v4 = v1.Add(v2);
Assert.AreEqual(5.0, v4.X, 1e-14);
Assert.AreEqual(7.0, v4.Y, 1e-14);
Assert.AreEqual(9.0, v4.Z, 1e-14);
}
