public Qauternion GetConjugate() { var result = new Qauternion(W, -X, -Y, -Z); result.Normalize(); return(result); }
private Vector3 RotateVector(Vector3 vector, Qauternion rotation) { var rotatedQ = new Qauternion(0, vector.X, vector.Y, vector.Z); Qauternion result = rotation * rotatedQ; result = result * rotation.GetConjugate(); return result.Axis; }
private Vector3 RotateVector(Vector3 vector, Qauternion rotation) { var rotatedQ = new Qauternion(0, vector.X, vector.Y, vector.Z); Qauternion result = rotation * rotatedQ; result = result * rotation.GetConjugate(); return(result.Axis); }
public Vector3 RotateVectorAroundAxis(Vector3 rotatingVector, Vector3 rotationVector, double angle) { if (Math.Abs(angle) < Consts.DoubleEqualityThreshold) { return new Vector3(rotatingVector.X, rotatingVector.Y, rotatingVector.Z); } var rotationQ = new Qauternion(rotationVector, angle); return RotateVector(rotatingVector, rotationQ); }
public Vector3 RotateVectorAroundAxis(Vector3 rotatingVector, Vector3 rotationVector, double angle) { if (Math.Abs(angle) < Consts.DoubleEqualityThreshold) { return(new Vector3(rotatingVector.X, rotatingVector.Y, rotatingVector.Z)); } var rotationQ = new Qauternion(rotationVector, angle); return(RotateVector(rotatingVector, rotationQ)); }
public Vector3[] GetLocalAxis(Vector3 vector, double adjustmentAngle, double referenceAngle) { double horizontalProjectionLength = Math.Sqrt(Math.Pow(vector.X, 2) + Math.Pow(vector.Y, 2)); bool isVertical = Math.Abs(horizontalProjectionLength) < Consts.DoubleEqualityThreshold; var result = new Vector3[3]; var rotationVector = Vector3.Up; if (isVertical) { // In this case up vector lies on X axis. // We rotate X unit vector around vertical axis to get vertical result double localXAxisAngle = referenceAngle + Math.PI / 2; var rotation1 = new Qauternion(rotationVector, localXAxisAngle); result[1] = RotateVector(Vector3.Right, rotation1); result[0] = vector.Clone(); //No need for adjustment in this case. } else { //Construct horizontal projection vector. var horizontalProjection = new Vector3(vector.X, vector.Y, 0); horizontalProjection.Normalize(); // Now we have a projection on XY plane of the initial vector. We need to first rotate it 90 degrees around Z axis to find the right vector. var rotation1 = new Qauternion(rotationVector, Math.PI / 2); result[1] = RotateVector(horizontalProjection, rotation1); // If adjustment is required we now rotate original vector by the required angle. // We need to rotate it closer to the horizontal plane. result[0] = AdjustVectorAsNeeded(vector, result[1], adjustmentAngle); double adjustedAngle = GetAngleBetweenVectors(result[0], horizontalProjection) * 180 / Math.PI; } // And then we can rotate that vector by 90 degrees around the right vector to get our Up vector. var rotation2 = new Qauternion(result[0], Math.PI / 2); result[2] = RotateVector(result[1], rotation2); if (Math.Abs(90 - GetAngleBetweenVectors(result[0], result[1]) * 180 / Math.PI) > Consts.DoubleEqualityThreshold || Math.Abs(90 - GetAngleBetweenVectors(result[1], result[2]) * 180 / Math.PI) > Consts.DoubleEqualityThreshold || Math.Abs(90 - GetAngleBetweenVectors(result[0], result[2]) * 180 / Math.PI) > Consts.DoubleEqualityThreshold) { throw new ArithmeticException("After trying to find local coordinate axes they were not 90 degrees away from the each other."); } return(result); }
public Vector3[] GetLocalAxis(Vector3 vector, double adjustmentAngle, double referenceAngle) { double horizontalProjectionLength = Math.Sqrt(Math.Pow(vector.X, 2) + Math.Pow(vector.Y, 2)); bool isVertical = Math.Abs(horizontalProjectionLength) < Consts.DoubleEqualityThreshold; var result = new Vector3[3]; var rotationVector = Vector3.Up; if (isVertical) { // In this case up vector lies on X axis. // We rotate X unit vector around vertical axis to get vertical result double localXAxisAngle = referenceAngle + Math.PI / 2; var rotation1 = new Qauternion(rotationVector, localXAxisAngle); result[1] = RotateVector(Vector3.Right, rotation1); result[0] = vector.Clone(); //No need for adjustment in this case. } else { //Construct horizontal projection vector. var horizontalProjection = new Vector3(vector.X, vector.Y, 0); horizontalProjection.Normalize(); // Now we have a projection on XY plane of the initial vector. We need to first rotate it 90 degrees around Z axis to find the right vector. var rotation1 = new Qauternion(rotationVector, Math.PI / 2); result[1] = RotateVector(horizontalProjection, rotation1); // If adjustment is required we now rotate original vector by the required angle. // We need to rotate it closer to the horizontal plane. result[0] = AdjustVectorAsNeeded(vector, result[1], adjustmentAngle); double adjustedAngle = GetAngleBetweenVectors(result[0], horizontalProjection) * 180 / Math.PI; } // And then we can rotate that vector by 90 degrees around the right vector to get our Up vector. var rotation2 = new Qauternion(result[0], Math.PI / 2); result[2] = RotateVector(result[1], rotation2); if (Math.Abs(90 - GetAngleBetweenVectors(result[0], result[1]) * 180 / Math.PI) > Consts.DoubleEqualityThreshold || Math.Abs(90 - GetAngleBetweenVectors(result[1], result[2]) * 180 / Math.PI) > Consts.DoubleEqualityThreshold || Math.Abs(90 - GetAngleBetweenVectors(result[0], result[2]) * 180 / Math.PI) > Consts.DoubleEqualityThreshold) { throw new ArithmeticException("After trying to find local coordinate axes they were not 90 degrees away from the each other."); } return result; }
protected bool Equals(Qauternion other) { return _w.Equals(other._w) && _x.Equals(other._x) && _y.Equals(other._y) && _z.Equals(other._z); }
public Qauternion GetConjugate() { var result = new Qauternion(W, -X, -Y, -Z); result.Normalize(); return result; }
protected bool Equals(Qauternion other) { return(_w.Equals(other._w) && _x.Equals(other._x) && _y.Equals(other._y) && _z.Equals(other._z)); }