public static FrustumPlane createByPointOnPlaneAndNormal(SpatialVectorDouble position, SpatialVectorDouble normal)
{
FrustumPlane resultPlane = new FrustumPlane();
resultPlane.normal = normal.deepClone();
resultPlane.distance = -SpatialVectorDouble.dot(normal, position);
return(resultPlane);
}
public bool checkIntersectPosition(SpatialVectorDouble position)
{
for (int i = 0; i < k / 2; i++)
{
double dotOfBaseVectorWithPosition = SpatialVectorDouble.dot(baseVectors[i], position);
if (min[i] > dotOfBaseVectorWithPosition || max[i] < dotOfBaseVectorWithPosition)
{
return(false);
}
}
return(true);
}
public EnumNonPreemptiveTaskState process()
{
bool objectExists;
PhysicsComponent controlledObject = tryGetControlledObject(out objectExists);
if (!objectExists)
{
return(EnumNonPreemptiveTaskState.FINISHEDSUCCESSFUL); // we finished if the object we have to align doesn't exist anymore
}
SpatialVectorDouble
forwardVector = controlledObject.forwardVector,
upVector = controlledObject.upVector,
sideVector = controlledObject.sideVector;
double
dotOfUpVectorAndTargetDirection = SpatialVectorDouble.dot(upVector, targetDirection),
dotOfSideVectorAndTargetDirection = SpatialVectorDouble.dot(sideVector, targetDirection),
dotOfForwardVectorAndTargetDirection = SpatialVectorDouble.dot(forwardVector, targetDirection);
//if (processingBegun) {
//pitchPid.reset(dotOfUpVectorAndTargetDirection);
//yawPid.reset(dotOfSideVectorAndTargetDirection);
//}
//processingBegun = false;
// the dot product results are like our rotation delta of the different axis
// now we need to put these into our PID's for the different axis to get the control value(s)
double currentPitchDerivative, currentYawDerivative;
double pitchControl = pitchPid.step(dotOfUpVectorAndTargetDirection, dt, out currentPitchDerivative);
double yawControl = yawPid.step(dotOfSideVectorAndTargetDirection, dt, out currentYawDerivative);
// send it to the controller
controller.inputPitch = (float)pitchControl;
controller.inputYaw = (float)yawControl;
// check for termination criterium of this Command
if (Math.dist2FromZero(currentPitchDerivative, currentYawDerivative) < targetDerivationDistance)
{
return(EnumNonPreemptiveTaskState.FINISHEDSUCCESSFUL);
}
return(EnumNonPreemptiveTaskState.INPROGRESS);
}
public static KDop calculateKdopFromVerticesAndbaseVectors(IList <SpatialVectorDouble> vertices, SpatialVectorDouble[] baseVectors, bool baseVectorsStartWithAabb)
{
uint k = (uint)baseVectors.Length * 2;
KDop result = new KDop(baseVectors, k, baseVectorsStartWithAabb);
foreach (SpatialVectorDouble iterationVertex in vertices)
{
for (int baseVectorI = 0; baseVectorI < baseVectors.Length; baseVectorI++)
{
double dotWithIterationVector = SpatialVectorDouble.dot(iterationVertex, baseVectors[baseVectorI]);
result.min[baseVectorI] = System.Math.Min(result.min[baseVectorI], dotWithIterationVector);
result.max[baseVectorI] = System.Math.Max(result.max[baseVectorI], dotWithIterationVector);
}
}
return(result);
}
// tests if a sphere is inside the frustum
public EnumFrustumIntersectionResult calcContainsForSphere(FrustumSphere sphere)
{
for (int i = 0; i < planes.Length; i++)
{
// find the distance to this plane
double distance = SpatialVectorDouble.dot(planes[i].normal, sphere.position) + planes[i].distance;
if (distance < -sphere.radius)
{
return(EnumFrustumIntersectionResult.OUTSIDE);
}
if (System.Math.Abs(distance) < sphere.radius)
{
return(EnumFrustumIntersectionResult.INTERSECT);
}
}
return(EnumFrustumIntersectionResult.INSIDE);
}
// generalized way to rotate in a specific direction
public void controlSolve(PhysicsComponent @object, float roll, float pitch, float yaw)
{
IList <ThrusterResponsibility.ThrusterBinding> thrusterBindings;
if (!thrusterResponsibility.physicsObjectIdToThrusters.TryGetValue(@object.id, out thrusterBindings))
{
return;
}
SpatialVectorDouble rotationTargetVector = new SpatialVectorDouble(new double[] { roll, yaw, pitch }); // vulkan coordinate system rotation
double maximalAngularAccelerationMagnitude = 0.0;
foreach (var iThrusterBinding in thrusterBindings)
{
maximalAngularAccelerationMagnitude = System.Math.Max(maximalAngularAccelerationMagnitude, iThrusterBinding.additionalInformation.cachedAngularAccelerationOnObject.length);
}
foreach (var iThrusterBinding in thrusterBindings)
{
SpatialVectorDouble normalizedAngularAccelerationOnObject;
if (iThrusterBinding.additionalInformation.cachedAngularAccelerationOnObject.length < double.Epsilon)
{
normalizedAngularAccelerationOnObject = new SpatialVectorDouble(new double[] { 0, 0, 0 });
}
else
{
SpatialVectorDouble cachedAngularAccelerationOnObject = iThrusterBinding.additionalInformation.cachedAngularAccelerationOnObject;
double normalizedMangitude = cachedAngularAccelerationOnObject.length / maximalAngularAccelerationMagnitude;
normalizedAngularAccelerationOnObject = cachedAngularAccelerationOnObject.normalized().scale(normalizedMangitude);
}
// calculate the relative thrust by the dot product because we want to rotate the best way in the wished rotation acceleration (given by roll, pitch, yaw)
double relativeThrust = SpatialVectorDouble.dot(normalizedAngularAccelerationOnObject, rotationTargetVector);
relativeThrust = System.Math.Max(relativeThrust, 0); // thruster can only thrust positivly
iThrusterBinding.relative += (float)relativeThrust; // now we add to not cancel away the effects of the other thrusters
}
}
static void applyForceToLinearAndAngularVelocity(PhysicsComponent physicsComponent, SpatialVectorDouble localForce, SpatialVectorDouble objectLocalPositionOfForce)
{
{ // linear part
// to calculate the linear component we use the dot product
double scaleOfLinearForce = 0.0;
if (localForce.length > double.Epsilon)
{
double dotOfForceAndLocalPosition = SpatialVectorDouble.dot(localForce.normalized(), objectLocalPositionOfForce.normalized());
scaleOfLinearForce = System.Math.Abs(dotOfForceAndLocalPosition);
}
// the linear force (and resulting acceleration) is the force scaled by the dot product
Matrix rotationMatrix = physicsComponent.calcLocalToGlobalRotationMatrix();
Matrix globalForceAsMatrix = rotationMatrix * SpatialVectorUtilities.toVector4(localForce).asMatrix;
SpatialVectorDouble globalForce = SpatialVectorUtilities.toVector3(new SpatialVectorDouble(globalForceAsMatrix));
physicsComponent.linearAcceleration += globalForce.scale(scaleOfLinearForce * physicsComponent.invMass);
}
{ // angular part
physicsComponent.eulerAngularAcceleration += physicsComponent.calcAngularAccelerationOfRigidBodyForAppliedForce(objectLocalPositionOfForce, localForce);
}
}
// see https://en.wikipedia.org/wiki/Line–plane_intersection#Algebraic_form
public static double calcD(SpatialVectorDouble p0, SpatialVectorDouble n, SpatialVectorDouble rayOrigin, SpatialVectorDouble rayDirection)
{
return(SpatialVectorDouble.dot((p0 - rayOrigin), n) / SpatialVectorDouble.dot(rayDirection, n));
}
// calculate the w value of a plane, p0 is a point on the plane
public static double calcW(SpatialVectorDouble p0, SpatialVectorDouble n)
{
return(SpatialVectorDouble.dot(p0, n));
}
// see http://slidegur.com/doc/1106443/plucker-coordinate slide 46
// "inside" if U1.V2 + V1.U2 > 0
static public bool checkCcw(PlueckerCoordinate c1, PlueckerCoordinate c2)
{
return(SpatialVectorDouble.dot(c1.u, c2.v) + SpatialVectorDouble.dot(c1.v, c2.u) > (double)0);
}
public static double planePointDistance(SpatialVectorDouble planeNormal, double planeD, SpatialVectorDouble point)
{
return(SpatialVectorDouble.dot(planeNormal, point) + planeD);
}