context: using System;
  
  namespace Tilar.Framework.Math
  {
      public class Quaterniond
      {
  
          private const long serialVersionUID = 1L;
  
          public double x;
          public double y;
          public double z;
          public double w;
  
          public Quaterniond()
          {
              x = 0.0;
              y = 0.0;
              z = 0.0;
              w = 1.0;
          }
          public Quaterniond(double x, double y, double z, double w)
          {
              this.x = x;
              this.y = y;
              this.z = z;
              this.w = w;
          }
          public Quaterniond(double x, double y, double z)
          {
              this.x = x;
              this.y = y;
              this.z = z;
              this.w = 1.0;
          }
          public Quaterniond(Quaterniond source)
          {
              x = source.x;
              y = source.y;
              z = source.z;
              w = source.w;
          }
          public Quaterniond(AxisAngle4d axisAngle)
          {
              double s = System.Math.Sin(axisAngle.angle * 0.5);
              x = axisAngle.x * s;
              y = axisAngle.y * s;
              z = axisAngle.z * s;
              w = System.Math.Cos(axisAngle.angle * 0.5);
          }
          public Quaterniond normalize()
          {
              double invNorm = 1.0 / System.Math.Sqrt(x * x + y * y + z * z + w * w);
              x *= invNorm;
              y *= invNorm;
              z *= invNorm;
              w *= invNorm;
              return this;
          }
          public Quaterniond normalize(Quaterniond dest)
          {
              double invNorm = 1.0 / System.Math.Sqrt(x * x + y * y + z * z + w * w);
              dest.x = x * invNorm;
              dest.y = y * invNorm;
              dest.z = z * invNorm;
              dest.w = w * invNorm;
              return dest;
          }
          public Quaterniond add(Quaterniond q2)
          {
              x += q2.x;
              y += q2.y;
              z += q2.z;
              w += q2.w;
              return this;
          }
          public Quaterniond add(Quaterniond q2, Quaterniond dest)
          {
              dest.x = x + q2.x;
              dest.y = y + q2.y;
              dest.z = z + q2.z;
              dest.w = w + q2.w;
              return dest;
          }
          public double dot(Quaterniond otherQuat)
          {
              return this.x * otherQuat.x + this.y * otherQuat.y + this.z * otherQuat.z + this.w * otherQuat.w;
          }
          public double angle()
          {
              double angle = 2.0 * System.Math.Acos(w);
              return angle <= System.Math.PI ? angle : 2.0 * System.Math.PI - angle;
          }
          public Matrix3d get(Matrix3d dest)
          {
              double q00 = 2.0 * x * x;
              double q11 = 2.0 * y * y;
              double q22 = 2.0 * z * z;
  
              double q01 = 2.0 * x * y;
              double q02 = 2.0 * x * z;
              double q03 = 2.0 * x * w;
  
              double q12 = 2.0 * y * z;
              double q13 = 2.0 * y * w;
  
              double q23 = 2.0 * z * w;
  
              dest.m00 = 1.0 - q11 - q22;
              dest.m01 = q01 + q23;
              dest.m02 = q02 - q13;
              dest.m10 = q01 - q23;
              dest.m11 = 1.0 - q22 - q00;
              dest.m12 = q12 + q03;
              dest.m20 = q02 + q13;
              dest.m21 = q12 - q03;
              dest.m22 = 1.0 - q11 - q00;
              return dest;
          }
          public Matrix4d get(Matrix4d dest)
          {
              double q00 = 2.0 * x * x;
              double q11 = 2.0 * y * y;
              double q22 = 2.0 * z * z;
  
              double q01 = 2.0 * x * y;
              double q02 = 2.0 * x * z;
              double q03 = 2.0 * x * w;
  
              double q12 = 2.0 * y * z;
              double q13 = 2.0 * y * w;
  
              double q23 = 2.0 * z * w;
  
              dest.m00 = 1.0 - q11 - q22;
              dest.m01 = q01 + q23;
              dest.m02 = q02 - q13;
              dest.m03 = 0.0;
              dest.m10 = q01 - q23;
              dest.m11 = 1.0 - q22 - q00;
              dest.m12 = q12 + q03;
              dest.m13 = 0.0;
              dest.m20 = q02 + q13;
              dest.m21 = q12 - q03;
              dest.m22 = 1.0 - q11 - q00;
              dest.m30 = 0.0;
              dest.m31 = 0.0;
              dest.m32 = 0.0;
              dest.m33 = 1.0;
              return dest;
          }
          public Quaterniond get(Quaterniond dest)
          {
              return dest.set(this);
          }
          public Quaterniond set(double x, double y, double z, double w)
          {
              this.x = x;
              this.y = y;
              this.z = z;
              this.w = w;
              return this;
          }
          public Quaterniond set(double x, double y, double z)
          {
              this.x = x;
              this.y = y;
              this.z = z;
              return this;
          }
          public Quaterniond set(Quaterniond q)
          {
              x = q.x;
              y = q.y;
              z = q.z;
              w = q.w;
              return this;
          }
          public Quaterniond set(AxisAngle4d axisAngle)
          {
              return setAngleAxis(axisAngle.angle, axisAngle.x, axisAngle.y, axisAngle.z);
          }
          public Quaterniond setAngleAxis(double angle, double x, double y, double z)
          {
              double s = System.Math.Sin(angle * 0.5);
              this.x = x * s;
              this.y = y * s;
              this.z = z * s;
              this.w = System.Math.Cos(angle * 0.5);
              return this;
          }
          public Quaterniond setAngleAxis(double angle, Vector3d axis)
          {
              return setAngleAxis(angle, axis.x, axis.y, axis.z);
          }
          private void setFromUnnormalized(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22)
          {
              double nm00 = m00, nm01 = m01, nm02 = m02;
              double nm10 = m10, nm11 = m11, nm12 = m12;
              double nm20 = m20, nm21 = m21, nm22 = m22;
              double lenX = 1.0 / System.Math.Sqrt(m00 * m00 + m01 * m01 + m02 * m02);
              double lenY = 1.0 / System.Math.Sqrt(m10 * m10 + m11 * m11 + m12 * m12);
              double lenZ = 1.0 / System.Math.Sqrt(m20 * m20 + m21 * m21 + m22 * m22);
              nm00 *= lenX; nm01 *= lenX; nm02 *= lenX;
              nm10 *= lenY; nm11 *= lenY; nm12 *= lenY;
              nm20 *= lenZ; nm21 *= lenZ; nm22 *= lenZ;
              setFromNormalized(nm00, nm01, nm02, nm10, nm11, nm12, nm20, nm21, nm22);
          }
  
          private void setFromNormalized(double m00, double m01, double m02, double m10, double m11, double m12, double m20, double m21, double m22)
          {
              double t;
              double tr = m00 + m11 + m22;
              if (tr >= 0.0)
              {
                  t = System.Math.Sqrt(tr + 1.0);
                  w = t * 0.5;
                  t = 0.5 / t;
                  x = (m12 - m21) * t;
                  y = (m20 - m02) * t;
                  z = (m01 - m10) * t;
              }
              else {
                  if (m00 >= m11 && m00 >= m22)
                  {
                      t = System.Math.Sqrt(m00 - (m11 + m22) + 1.0);
                      x = t * 0.5;
                      t = 0.5 / t;
                      y = (m10 + m01) * t;
                      z = (m02 + m20) * t;
                      w = (m12 - m21) * t;
                  }
                  else if (m11 > m22)
                  {
                      t = System.Math.Sqrt(m11 - (m22 + m00) + 1.0);
                      y = t * 0.5;
                      t = 0.5 / t;
                      z = (m21 + m12) * t;
                      x = (m10 + m01) * t;
                      w = (m20 - m02) * t;
                  }
                  else {
                      t = System.Math.Sqrt(m22 - (m00 + m11) + 1.0);
                      z = t * 0.5;
                      t = 0.5 / t;
                      x = (m02 + m20) * t;
                      y = (m21 + m12) * t;
                      w = (m01 - m10) * t;
                  }
              }
          }
          public Quaterniond setFromUnnormalized(Matrix4d mat)
          {
              setFromUnnormalized(mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22);
              return this;
          }
          public Quaterniond setFromNormalized(Matrix4d mat)
          {
              setFromNormalized(mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22);
              return this;
          }
          public Quaterniond setFromUnnormalized(Matrix3d mat)
          {
              setFromUnnormalized(mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22);
              return this;
          }
          public Quaterniond setFromNormalized(Matrix3d mat)
          {
              setFromNormalized(mat.m00, mat.m01, mat.m02, mat.m10, mat.m11, mat.m12, mat.m20, mat.m21, mat.m22);
              return this;
          }
          public Quaterniond fromAxisAngleRad(Vector3d axis, double angle)
          {
              double hangle = angle / 2.0;
              double sinAngle = System.Math.Sin(hangle);
              double vLength = axis.length();
  
              x = axis.x / vLength * sinAngle;
              y = axis.y / vLength * sinAngle;
              z = axis.z / vLength * sinAngle;
              w = System.Math.Cos(hangle);
  
              return this;
          }
          public Quaterniond fromAxisAngleRad(double axisX, double axisY, double axisZ, double angle)
          {
              double hangle = angle / 2.0;
              double sinAngle = System.Math.Sin(hangle);
              double vLength = System.Math.Sqrt(axisX * axisX + axisY * axisY + axisZ * axisZ);
  
              x = axisX / vLength * sinAngle;
              y = axisY / vLength * sinAngle;
              z = axisZ / vLength * sinAngle;
              w = System.Math.Cos(hangle);
  
              return this;
          }
          public Quaterniond fromAxisAngleDeg(Vector3d axis, double angle)
          {
              double hangle = angle / 2.0;
              double sinAngle = System.Math.Sin(hangle);
              double vLength = axis.length();
  
              x = axis.x / vLength * sinAngle;
              y = axis.y / vLength * sinAngle;
              z = axis.z / vLength * sinAngle;
              w = System.Math.Cos(hangle);
  
              return this;
          }
          public Quaterniond mul(Quaterniond q)
          {
              return mul(q, this);
          }
          public Quaterniond mul(Quaterniond q, Quaterniond dest)
          {
              dest.set(w * q.x + x * q.w + y * q.z - z * q.y,
                       w * q.y - x * q.z + y * q.w + z * q.x,
                       w * q.z + x * q.y - y * q.x + z * q.w,
                       w * q.w - x * q.x - y * q.y - z * q.z);
              return dest;
          }
          public Quaterniond mul(double qx, double qy, double qz, double qw)
          {
              set(w * qx + x * qw + y * qz - z * qy,
                  w * qy - x * qz + y * qw + z * qx,
                  w * qz + x * qy - y * qx + z * qw,
                  w * qw - x * qx - y * qy - z * qz);
              return this;
          }
          public Quaterniond mul(double qx, double qy, double qz, double qw, Quaterniond dest)
          {
              dest.set(w * qx + x * qw + y * qz - z * qy,
                       w * qy - x * qz + y * qw + z * qx,
                       w * qz + x * qy - y * qx + z * qw,
                       w * qw - x * qx - y * qy - z * qz);
              return dest;
          }
          public Vector3d transform(Vector3d vec)
          {
              return transform(vec, vec);
          }
          public Vector3d transform(Vector3d vec, Vector3d dest)
          {
              double num = x * 2.0;
              double num2 = y * 2.0;
              double num3 = z * 2.0;
              double num4 = x * num;
              double num5 = y * num2;
              double num6 = z * num3;
              double num7 = x * num2;
              double num8 = x * num3;
              double num9 = y * num3;
              double num10 = w * num;
              double num11 = w * num2;
              double num12 = w * num3;
              dest.set((1.0 - (num5 + num6)) * vec.x + (num7 - num12) * vec.y + (num8 + num11) * vec.z,
                       (num7 + num12) * vec.x + (1.0 - (num4 + num6)) * vec.y + (num9 - num10) * vec.z,
                       (num8 - num11) * vec.x + (num9 + num10) * vec.y + (1.0 - (num4 + num5)) * vec.z);
              return dest;
          }
          public Vector4d transform(Vector4d vec)
          {
              return transform(vec, vec);
          }
          public Vector4d transform(Vector4d vec, Vector4d dest)
          {
              double num = x * 2.0;
              double num2 = y * 2.0;
              double num3 = z * 2.0;
              double num4 = x * num;
              double num5 = y * num2;
              double num6 = z * num3;
              double num7 = x * num2;
              double num8 = x * num3;
              double num9 = y * num3;
              double num10 = w * num;
              double num11 = w * num2;
              double num12 = w * num3;
              dest.set((1.0 - (num5 + num6)) * vec.x + (num7 - num12) * vec.y + (num8 + num11) * vec.z,
                       (num7 + num12) * vec.x + (1.0 - (num4 + num6)) * vec.y + (num9 - num10) * vec.z,
                       (num8 - num11) * vec.x + (num9 + num10) * vec.y + (1.0 - (num4 + num5)) * vec.z,
                       dest.w);
              return dest;
          }
          public Quaterniond invert(Quaterniond dest)
          {
              double invNorm = 1.0 / (x * x + y * y + z * z + w * w);
              dest.x = -x * invNorm;
              dest.y = -y * invNorm;
              dest.z = -z * invNorm;
              dest.w = w * invNorm;
              return dest;
          }
          public Quaterniond unitInvert(Quaterniond dest)
          {
              dest.x = -x;
              dest.y = -y;
              dest.z = -z;
              dest.w = w;
              return dest;
          }
          public Quaterniond invert()
          {
              return invert(this);
          }
          public Quaterniond unitInvert()
          {
              return unitInvert(this);
          }
          public Quaterniond div(Quaterniond b, Quaterniond dest)
          {
              double invNorm = 1.0 / (b.x * b.x + b.y * b.y + b.z * b.z + b.w * b.w);
              double x = -b.x * invNorm;
              double y = -b.y * invNorm;
              double z = -b.z * invNorm;
              double w = b.w * invNorm;
              dest.set(this.w * x + this.x * w + this.y * z - this.z * y,
                       this.w * y - this.x * z + this.y * w + this.z * x,
                       this.w * z + this.x * y - this.y * x + this.z * w,
                       this.w * w - this.x * x - this.y * y - this.z * z);
              return dest;
          }
          public Quaterniond div(Quaterniond b)
          {
              return div(b, this);
          }
          public Quaterniond conjugate()
          {
              x = -x;
              y = -y;
              z = -z;
              return this;
          }
          public Quaterniond conjugate(Quaterniond dest)
          {
              dest.x = -x;
              dest.y = -y;
              dest.z = -z;
              dest.w = w;
              return dest;
          }
          public Quaterniond identity()
          {
              x = 0.0;
              y = 0.0;
              z = 0.0;
              w = 1.0;
              return this;
          }
          public double lengthSquared()
          {
              return x * x + y * y + z * z + w * w;
          }
          public Quaterniond rotationXYZ(double angleX, double angleY, double angleZ)
          {
              double sx = System.Math.Sin(angleX * 0.5);
              double cx = System.Math.Cos(angleX * 0.5);
              double sy = System.Math.Sin(angleY * 0.5);
              double cy = System.Math.Cos(angleY * 0.5);
              double sz = System.Math.Sin(angleZ * 0.5);
              double cz = System.Math.Cos(angleZ * 0.5);
  
              double cycz = cy * cz;
              double sysz = sy * sz;
              double sycz = sy * cz;
              double cysz = cy * sz;
              w = cx * cycz - sx * sysz;
              x = sx * cycz + cx * sysz;
              y = cx * sycz - sx * cysz;
              z = cx * cysz + sx * sycz;
  
              return this;
          }
          public Quaterniond rotationZYX(double angleZ, double angleY, double angleX)
          {
              double sx = System.Math.Sin(angleX * 0.5);
              double cx = System.Math.Cos(angleX * 0.5);
              double sy = System.Math.Sin(angleY * 0.5);
              double cy = System.Math.Cos(angleY * 0.5);
              double sz = System.Math.Sin(angleZ * 0.5);
              double cz = System.Math.Cos(angleZ * 0.5);
  
              double cycz = cy * cz;
              double sysz = sy * sz;
              double sycz = sy * cz;
              double cysz = cy * sz;
              w = cx * cycz + sx * sysz;
              x = sx * cycz - cx * sysz;
              y = cx * sycz + sx * cysz;
              z = cx * cysz - sx * sycz;
  
              return this;
          }
          public Quaterniond rotationYXZ(double angleY, double angleX, double angleZ)
          {
              double sx = System.Math.Sin(angleX * 0.5);
              double cx = System.Math.Cos(angleX * 0.5);
              double sy = System.Math.Sin(angleY * 0.5);
              double cy = System.Math.Cos(angleY * 0.5);
              double sz = System.Math.Sin(angleZ * 0.5);
              double cz = System.Math.Cos(angleZ * 0.5);
  
              double x = cy * sx;
              double y = sy * cx;
              double z = sy * sx;
              double w = cy * cx;
              this.x = x * cz + y * sz;
              this.y = y * cz - x * sz;
              this.z = w * sz - z * cz;
              this.w = w * cz + z * sz;
  
              return this;
          }
          public Quaterniond slerp(Quaterniond target, double alpha)
          {
              return slerp(target, alpha, this);
          }
          public Quaterniond slerp(Quaterniond target, double alpha, Quaterniond dest)
          {
              double cosom = x * target.x + y * target.y + z * target.z + w * target.w;
              double absCosom = System.Math.Abs(cosom);
              double scale0, scale1;
              if (1.0 - absCosom > 1E-6)
              {
                  double sinSqr = 1.0 - absCosom * absCosom;
                  double sinom = 1.0 / System.Math.Sqrt(sinSqr);
                  double omega = System.Math.Atan2(sinSqr * sinom, absCosom);
                  scale0 = System.Math.Sin((1.0 - alpha) * omega) * sinom;
                  scale1 = System.Math.Sin(alpha * omega) * sinom;
              }
              else {
                  scale0 = 1.0 - alpha;
                  scale1 = alpha;
              }
              scale1 = cosom >= 0.0 ? scale1 : -scale1;
              dest.x = scale0 * x + scale1 * target.x;
              dest.y = scale0 * y + scale1 * target.y;
              dest.z = scale0 * z + scale1 * target.z;
              dest.w = scale0 * w + scale1 * target.w;
              return dest;
          }
  
          /**
           * Compute a linear (non-spherical) interpolation of <code>this</code> and the given quaternion <code>q</code>
           * and store the result in <code>this</code>.
           * 
           * @param q
           *          the other quaternion
           * @param factor
           *          the interpolation factor. It is between 0.0 and 1.0
           * @return this
           */
          public Quaterniond nlerp(Quaterniond q, double factor)
          {
              return nlerp(q, factor, this);
          }
  
          /**
           * Compute a linear (non-spherical) interpolation of <code>this</code> and the given quaternion <code>q</code>
           * and store the result in <code>dest</code>.
           * <p>
           * Reference: <a href="http://fabiensanglard.net/doom3_documentation/37725-293747_293747.pdf">http://fabiensanglard.net</a>
           * 
           * @param q
           *          the other quaternion
           * @param factor
           *          the interpolation factor. It is between 0.0 and 1.0
           * @param dest
           *          will hold the result
           * @return dest
           */
          public Quaterniond nlerp(Quaterniond q, double factor, Quaterniond dest)
          {
              double cosom = x * q.x + y * q.y + z * q.z + w * q.w;
              double scale0 = 1.0 - factor;
              double scale1 = (cosom >= 0.0) ? factor : -factor;
              dest.x = scale0 * x + scale1 * q.x;
              dest.y = scale0 * y + scale1 * q.y;
              dest.z = scale0 * z + scale1 * q.z;
              dest.w = scale0 * w + scale1 * q.w;
              double s = 1.0 / System.Math.Sqrt(dest.x * dest.x + dest.y * dest.y + dest.z * dest.z + dest.w * dest.w);
              dest.x *= s;
              dest.y *= s;
              dest.z *= s;
              dest.w *= s;
              return dest;
          }
  
          /**
           * Compute linear (non-spherical) interpolations of <code>this</code> and the given quaternion <code>q</code>
           * iteratively and store the result in <code>dest</code>.
           * <p>
           * This method performs a series of small-step nlerp interpolations to avoid doing a costly spherical linear interpolation, like
           * {@link #slerp(Quaterniond, double, Quaterniond) slerp},
           * by subdividing the rotation arc between <code>this</code> and <code>q</code> via non-spherical linear interpolations as long as
           * the absolute dot product of <code>this</code> and <code>q</code> is greater than the given <code>dotThreshold</code> parameter.
           * <p>
           * Thanks to <tt>@theagentd</tt> at <a href="http://www.java-gaming.org/">http://www.java-gaming.org/</a> for providing the code.
           * 
           * @param q
           *          the other quaternion
           * @param alpha
           *          the interpolation factor, between 0.0 and 1.0
           * @param dotThreshold
           *          the threshold for the dot product of <code>this</code> and <code>q</code> above which this method performs another iteration
           *          of a small-step linear interpolation
           * @param dest
           *          will hold the result
           * @return dest
           */
          public Quaterniond nlerpIterative(Quaterniond q, double alpha, double dotThreshold, Quaterniond dest)
          {
              double q1x = x, q1y = y, q1z = z, q1w = w;
              double q2x = q.x, q2y = q.y, q2z = q.z, q2w = q.w;
              double dot = q1x * q2x + q1y * q2y + q1z * q2z + q1w * q2w;
              double absDot = System.Math.Abs(dot);
              if (1.0 - 1E-6 < absDot)
              {
                  return dest.set(this);
              }
              double alphaN = alpha;
              while (absDot < dotThreshold)
              {
                  double scale0 = 0.5;
                  double scale1 = dot >= 0.0 ? 0.5 : -0.5;
                  if (alphaN < 0.5)
                  {
                      q2x = scale0 * q2x + scale1 * q1x;
                      q2y = scale0 * q2y + scale1 * q1y;
                      q2z = scale0 * q2z + scale1 * q1z;
                      q2w = scale0 * q2w + scale1 * q1w;
                      double s = 1.0 / System.Math.Sqrt(q2x * q2x + q2y * q2y + q2z * q2z + q2w * q2w);
                      q2x *= s;
                      q2y *= s;
                      q2z *= s;
                      q2w *= s;
                      alphaN = alphaN * 2.0;
                  }
                  else {
                      q1x = scale0 * q1x + scale1 * q2x;
                      q1y = scale0 * q1y + scale1 * q2y;
                      q1z = scale0 * q1z + scale1 * q2z;
                      q1w = scale0 * q1w + scale1 * q2w;
                      double s = 1.0 / System.Math.Sqrt(q1x * q1x + q1y * q1y + q1z * q1z + q1w * q1w);
                      q1x *= s;
                      q1y *= s;
                      q1z *= s;
                      q1w *= s;
                      alphaN = alphaN * 2.0 - 1.0;
                  }
                  dot = q1x * q2x + q1y * q2y + q1z * q2z + q1w * q2w;
                  absDot = System.Math.Abs(dot);
              }
              double scale0 = 1.0 - alphaN;
              double scale1 = dot >= 0.0 ? alphaN : -alphaN;
              double destX = scale0 * q1x + scale1 * q2x;
              double destY = scale0 * q1y + scale1 * q2y;
              double destZ = scale0 * q1z + scale1 * q2z;
              double destW = scale0 * q1w + scale1 * q2w;
              double s = 1.0 / System.Math.Sqrt(destX * destX + destY * destY + destZ * destZ + destW * destW);
              dest.x *= s;
              dest.y *= s;
              dest.z *= s;
              dest.w *= s;
              return dest;
          }
          public Quaterniond lookRotate(Vector3d dir, Vector3d up)
          {
              return lookRotate(dir.x, dir.y, dir.z, up.x, up.y, up.z, this);
          }
          public Quaterniond lookRotate(Vector3d dir, Vector3d up, Quaterniond dest)
          {
              return lookRotate(dir.x, dir.y, dir.z, up.x, up.y, up.z, dest);
          }
          public Quaterniond lookRotate(double dirX, double dirY, double dirZ, double upX, double upY, double upZ)
          {
              return lookRotate(dirX, dirY, dirZ, upX, upY, upZ, this);
          }
          public Quaterniond lookRotate(double dirX, double dirY, double dirZ, double upX, double upY, double upZ, Quaterniond dest)
          {
              // Normalize direction
              double invDirLength = 1.0 / System.Math.Sqrt(dirX * dirX + dirY * dirY + dirZ * dirZ);
              double dirnX = dirX * invDirLength;
              double dirnY = dirY * invDirLength;
              double dirnZ = dirZ * invDirLength;
              // left = up x dir
              double leftX, leftY, leftZ;
              leftX = upY * dirnZ - upZ * dirnY;
              leftY = upZ * dirnX - upX * dirnZ;
              leftZ = upX * dirnY - upY * dirnX;
              // normalize left
              double invLeftLength = 1.0 / System.Math.Sqrt(leftX * leftX + leftY * leftY + leftZ * leftZ);
              leftX *= invLeftLength;
              leftY *= invLeftLength;
              leftZ *= invLeftLength;
              // up = direction x left
              double upnX = dirnY * leftZ - dirnZ * leftY;
              double upnY = dirnZ * leftX - dirnX * leftZ;
              double upnZ = dirnX * leftY - dirnY * leftX;
  
              /* Convert orthonormal basis vectors to quaternion */
              double x, y, z, w;
              double t;
              double tr = leftX + upnY + dirnZ;
              if (tr >= 0.0)
              {
                  t = System.Math.Sqrt(tr + 1.0);
                  w = t * 0.5;
                  t = 0.5 / t;
                  x = (dirnY - upnZ) * t;
                  y = (leftZ - dirnX) * t;
                  z = (upnX - leftY) * t;
              }
              else {
                  if (leftX > upnY && leftX > dirnZ)
                  {
                      t = System.Math.Sqrt(1.0 + leftX - upnY - dirnZ);
                      x = t * 0.5;
                      t = 0.5 / t;
                      y = (leftY + upnX) * t;
                      z = (dirnX + leftZ) * t;
                      w = (dirnY - upnZ) * t;
                  }
  
gt:                 else if (upnY > dirnZ)