// Rotate a vector.
// @param v the vector to be rotated
// @returns the resulting rotated vector
public Vector3D Rotate(Vector3D v)
{
float angle = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
if (angle < 1e-8)
{
return(v);
}
float sn = (float)Math.Sin(angle);
float cs = (float)Math.Cos(angle);
Vector3D axis = new Vector3D(_x / angle, _y / angle, _z / angle);
Vector3D n = new Vector3D(Point3D.VectorProduct(axis, v));
Vector3D delta = new Vector3D(sn * n + (cs - 1.0f) * (Point3D.VectorProduct(n, axis)));
return(new Vector3D(v + delta));
}
// Rotate a line segment.
// @param l the line segment to be rotated
// @returns the resulting rotated line segment
public LineSeg3D Rotate(LineSeg3D l)
{
// Simply rotate both lines
float angle = (float)Math.Sqrt(_x * _x + _y * _y + _z * _z);
if (angle < 1e-8)
{
return(l);
}
float sn = (float)Math.Sin(angle);
float cs = (float)Math.Cos(angle);
Vector3D axis = new Vector3D(_x / angle, _y / angle, _z / angle);
Vector3D ns = new Vector3D(Point3D.VectorProduct(axis, l.StartPoint()));
Vector3D ne = new Vector3D(Point3D.VectorProduct(axis, l.EndPoint()));
Vector3D deltas = new Vector3D(sn * ns + (cs - 1.0f) * (Point3D.VectorProduct(ns, axis)));
Vector3D deltae = new Vector3D(sn * ne + (cs - 1.0f) * (Point3D.VectorProduct(ne, axis)));
return(new LineSeg3D(l.StartPoint() + deltas, l.EndPoint() + deltae));
}
// Combine two rotations.
// @param q the inner rotation
// @returns the resulting rotation (like this*q for matrices)
public Quaternion Multiply(Quaternion q)
{
// real and img parts of args
float r1 = this._r;
float r2 = q._r;
Vector3D i1 = this.Imaginary();
Vector3D i2 = q.Imaginary();
// Real is product of real, and dot-product of imag
float real_v = (r1 * r2) - (i1 * i2);
// Imag is cross product of imag + real*imag for each
Point3D img = Point3D.VectorProduct(i1, i2);
//Point3D img2 = (img + (i2 * r1) + (i1 * r2));
// Finally, build the result
Quaternion prod = new Quaternion(img.GetX(), img.GetY(), img.GetZ(), real_v);
return(prod);
}