/// <summary>
/// Rotate this vector by an angle around an axis
/// </summary>
/// <param name="axis"></param>
/// <param name="angle"></param>
/// <returns></returns>
public FdVector3d RotateAroundAxis(double angle, FdVector3d axis)
{
// normalize vector
FdVector3d _axis = axis.Normalize();
// https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle
double[,] rotationMatrix = new double[3, 3];
rotationMatrix[0, 0] = Math.Cos(angle) + Math.Pow(_axis.X, 2) * (1 - Math.Cos(angle));
rotationMatrix[0, 1] = _axis.X * _axis.Y * (1 - Math.Cos(angle)) - _axis.Z * Math.Sin(angle);
rotationMatrix[0, 2] = _axis.X * _axis.Z * (1 - Math.Cos(angle)) + _axis.Y * Math.Sin(angle);
rotationMatrix[1, 0] = _axis.Y * _axis.X * (1 - Math.Cos(angle)) + _axis.Z * Math.Sin(angle);
rotationMatrix[1, 1] = Math.Cos(angle) + Math.Pow(_axis.Y, 2) * (1 - Math.Cos(angle));
rotationMatrix[1, 2] = _axis.Y * _axis.Z * (1 - Math.Cos(angle)) - _axis.X * Math.Sin(angle);
rotationMatrix[2, 0] = _axis.Z * _axis.X * (1 - Math.Cos(angle)) - _axis.Y * Math.Sin(angle);
rotationMatrix[2, 1] = _axis.Z * _axis.Y * (1 - Math.Cos(angle)) + _axis.X * Math.Sin(angle);
rotationMatrix[2, 2] = Math.Cos(angle) + Math.Pow(_axis.Z, 2) * (1 - Math.Cos(angle));
// matrix multiplication
double x = this.X * rotationMatrix[0, 0] + this.Y * rotationMatrix[0, 1] + this.Z * rotationMatrix[0, 2];
double y = this.X * rotationMatrix[1, 0] + this.Y * rotationMatrix[1, 1] + this.Z * rotationMatrix[1, 2];
double z = this.X * rotationMatrix[2, 0] + this.Y * rotationMatrix[2, 1] + this.Z * rotationMatrix[2, 2];
// return new vector
return(new FdVector3d(x, y, z));
}
/// <summary>
/// Check if this FdVector3d is parallel to v.
/// Returns 1 if parallel, -1 if antiparallel, 0 if not parallel
/// </summary>
public int Parallel(FdVector3d v)
{
FdVector3d v0 = this.Normalize();
FdVector3d v1 = v.Normalize();
if (v0.Equals(v1, Tolerance.Point3d))
{
return(1);
}
else if (v0.Scale(-1).Equals(v, Tolerance.Point3d))
{
return(-1);
}
else
{
return(0);
}
}
/// <summary>
/// Set Y-axis and rotate coordinate system accordingly around X-Axis.
/// </summary>
public void SetYAroundX(FdVector3d vector)
{
// try to set axis
FdVector3d val = vector.Normalize();
FdVector3d x = this.LocalX;
double dot = x.Dot(val);
if (Math.Abs(dot) < Tolerance.DotProduct)
{
this._localY = val;
this._localZ = x.Cross(val); // follows right-hand-rule
}
else
{
throw new System.ArgumentException($"The passed Y-axis is not perpendicular to X-axis. The dot-product is {dot}, but should be 0");
}
}
