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