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