matrix involved in quaternion rotation
437185
def makeW(r1, r2, r3, r4=0):
    
    W = np.asarray([
        [r4, r3, -r2, r1],
        [-r3, r4, r1, r2],
        [r2, -r1, r4, r3],
        [-r1, -r2, -r3, r4]])
    return W
304976
def quaternion(self):
        
        q_xyzw = transformations.quaternion_from_matrix(self.matrix)
        q_wxyz = np.roll(q_xyzw, 1)
        return q_wxyz
526102
function _fromEuler(x, y, z) {
  x = x / 2;
  y = y / 2;
  z = z / 2;

  var c1 = Math.cos(y),
      s1 = Math.sin(y),
      c2 = Math.cos(x),
      s2 = Math.sin(x),
      c3 = Math.cos(z),
      s3 = Math.sin(z);

  return new _Quaternion(
      s1 * c2 * s3 + c1 * s2 * c3,
      s1 * c2 * c3 - c1 * s2 * s3,
      c1 * c2 * s3 - s1 * s2 * c3,
      c1 * c2 * c3 + s1 * s2 * s3);
}
954059
def quat_rotate(rotation, vector):
    

    def quat_mult_point(q, w):
        return (q[3] * w[0] + q[1] * w[2] - q[2] * w[1],
                q[3] * w[1] + q[2] * w[0] - q[0] * w[2],
                q[3] * w[2] + q[0] * w[1] - q[1] * w[0],
                -q[0] * w[0] - q[1] * w[1] - q[2] * w[2])

    q = quat_mult_point(rotation, vector)
    q = tf.transformations.quaternion_multiply(
        q, tf.transformations.quaternion_inverse(rotation))
    return [q[0], q[1], q[2]]
377988
def _q_matrix(self):
        
        return np.array([
            [self.q[0], -self.q[1], -self.q[2], -self.q[3]],
            [self.q[1],  self.q[0], -self.q[3],  self.q[2]],
            [self.q[2],  self.q[3],  self.q[0], -self.q[1]],
            [self.q[3], -self.q[2],  self.q[1],  self.q[0]]])
216205
def rotvec2mat(u, phi):
    """"""

    phi = np.squeeze(phi)
    norm_u = np.linalg.norm(u)

    if norm_u < 1e-12:
        raise Exception("the rotation vector is equal to zero")

    u = u / norm_u
    # http://en.wikipedia.org/wiki/Rotation_matrix
    s = np.sin(phi)
    c = np.cos(phi)
    t = 1 - c

    ux = u[0]
    uy = u[1]
    uz = u[2]
    res = np.array([[t * ux * ux + c, t * ux * uy - s * uz, t * ux * uz + s * uy],
                    [t * ux * uy + s * uz, t * uy * uy + c, t * uy * uz - s * ux],
                    [t * ux * uz - s * uy, t * uy * uz + s * ux, t * uz * uz + c]])

    return res
609634
def inverse_transform(t):
    
    return [
        quat_rotate(
            tf.transformations.quaternion_inverse(t[1]),
            [-t[0][0], -t[0][1], -t[0][2]]),
        tf.transformations.quaternion_inverse(t[1])
    ]
110054
def set_from_matrix44(self, mat):
        

        # Matrix trace
        trace = mat.data[0][0] + mat.data[1][1] + mat.data[2][2] + 1.0

        if trace > 0.00000001:
            # n4 is norm of quaternion multiplied by 4.
            n4 = math.sqrt(trace) * 2
            self.x = (mat.data[1][2] - mat.data[2][1]) / n4
            self.y = (mat.data[2][0] - mat.data[0][2]) / n4
            self.z = (mat.data[0][1] - mat.data[1][0]) / n4
            self.w = n4 / 4.0
            return self

        # TODO: unittests for code below when trace is small.

        # matrix trace <= 0
        if mat.data[0][0] > mat.data[1][1] and mat.data[0][0] > mat.data[2][2]:
            s = 2.0 * math.sqrt(1.0 + mat.data[0][0] - mat.data[1][1] -
                                mat.data[2][2])
            self.x = s / 4.0
            self.y = (mat.data[1][0] + mat.data[0][1]) / s
            self.z = (mat.data[2][0] + mat.data[0][2]) / s
            self.w = (mat.data[2][1] - mat.data[1][2]) / s
            return self
        elif mat.data[1][1] > mat.data[2][2]:
            s = 2.0 * math.sqrt(1.0 - mat.data[0][0] + mat.data[1][1] -
                                mat.data[2][2])
            self.x = (mat.data[1][0] + mat.data[0][1]) / s
            self.y = s / 4.0
            self.z = (mat.data[2][1] + mat.data[1][2]) / s
            self.w = (mat.data[2][0] - mat.data[0][2]) / s
            return self
        else:
            s = 2.0 * math.sqrt(1.0 - mat.data[0][0] - mat.data[1][1] +
                                mat.data[2][2])
            self.x = (mat.data[2][0] + mat.data[0][2]) / s
            self.y = (mat.data[2][1] + mat.data[1][2]) / s
            self.z = s / 4.0
            self.w = (mat.data[1][0] - mat.data[0][1]) / s
            return self
490346
function() {

      // Q* := Q / |Q|

      // unrolled Q.scale(1 / Q.norm())

      var w = this['w'];
      var x = this['x'];
      var y = this['y'];
      var z = this['z'];

      var norm = Math.sqrt(w * w + x * x + y * y + z * z);

      if (norm < Quaternion['EPSILON']) {
        return Quaternion['ZERO'];
      }

      norm = 1 / norm;

      return new Quaternion(w * norm, x * norm, y * norm, z * norm);
    }
33747
def rotated(self, angle_degrees_ccw):
        """"""
        angle = angle_degrees_ccw / 180.0 * pi
        c, s = cos(angle), sin(angle)
        return self @ PdfMatrix((c, s, -s, c, 0, 0))
119618
def norm(self):
        
        qr_c = quaternion_conjugate(self._qr)
        qd_c = quaternion_conjugate(self._qd)

        qr_norm = np.linalg.norm(quaternion_multiply(self._qr, qr_c))
        qd_norm = np.linalg.norm(quaternion_multiply(self._qr, qd_c) + quaternion_multiply(self._qd, qr_c))

        return (qr_norm, qd_norm)
1027708
public Quaterniond rotateTo(double fromDirX, double fromDirY, double fromDirZ,
                                double toDirX, double toDirY, double toDirZ, Quaterniond dest) {
        double x = fromDirY * toDirZ - fromDirZ * toDirY;
        double y = fromDirZ * toDirX - fromDirX * toDirZ;
        double z = fromDirX * toDirY - fromDirY * toDirX;
        double w = Math.sqrt((fromDirX * fromDirX + fromDirY * fromDirY + fromDirZ * fromDirZ) *
                                    (toDirX * toDirX + toDirY * toDirY + toDirZ * toDirZ)) +
                  (fromDirX * toDirX + fromDirY * toDirY + fromDirZ * toDirZ);
        double invNorm = 1.0 / Math.sqrt(x * x + y * y + z * z + w * w);
        if (Double.isInfinite(invNorm)) {
            // Rotation is ambiguous: Find appropriate rotation axis (1. try toDir x +Z)
            x = toDirY; y = -toDirX; z = 0.0; w = 0.0;
            invNorm = (float) (1.0 / Math.sqrt(x * x + y * y));
            if (Double.isInfinite(invNorm)) {
                // 2. try toDir x +X
                x = 0.0; y = toDirZ; z = -toDirY; w = 0.0;
                invNorm = (float) (1.0 / Math.sqrt(y * y + z * z));
            }
        }
        x *= invNorm;
        y *= invNorm;
        z *= invNorm;
        w *= invNorm;
        /* Multiply */
        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;
    }
134763
def quaternion_about_axis(angle, axis):
    
    q = np.array([0.0, axis[0], axis[1], axis[2]])
    qlen = vector_norm(q)
    if qlen > _EPS:
        q *= math.sin(angle / 2.0) / qlen
    q[0] = math.cos(angle / 2.0)
    return q
1161711
public Matrix4f rotateAround(Quaternionfc quat, float ox, float oy, float oz, Matrix4f dest) {
        if ((properties & PROPERTY_IDENTITY) != 0)
            return rotationAround(quat, ox, oy, oz);
        else if ((properties & PROPERTY_AFFINE) != 0)
            return rotateAroundAffine(quat, ox, oy, oz, dest);
        return rotateAroundGeneric(quat, ox, oy, oz, dest);
    }
394310
function transformQuat(out, a, q) {
  // benchmarks: https://jsperf.com/quaternion-transform-vec3-implementations-fixed
  var qx = q[0],
      qy = q[1],
      qz = q[2],
      qw = q[3];
  var x = a[0],
      y = a[1],
      z = a[2];
  // var qvec = [qx, qy, qz];
  // var uv = vec3.cross([], qvec, a);
  var uvx = qy * z - qz * y,
      uvy = qz * x - qx * z,
      uvz = qx * y - qy * x;
  // var uuv = vec3.cross([], qvec, uv);
  var uuvx = qy * uvz - qz * uvy,
      uuvy = qz * uvx - qx * uvz,
      uuvz = qx * uvy - qy * uvx;
  // vec3.scale(uv, uv, 2 * w);
  var w2 = qw * 2;
  uvx *= w2;
  uvy *= w2;
  uvz *= w2;
  // vec3.scale(uuv, uuv, 2);
  uuvx *= 2;
  uuvy *= 2;
  uuvz *= 2;
  // return vec3.add(out, a, vec3.add(out, uv, uuv));
  out[0] = x + uvx + uuvx;
  out[1] = y + uvy + uuvy;
  out[2] = z + uvz + uuvz;
  return out;
}
825631
def rotation_matrix_from_axis_and_angle(u, theta):
    ''''''
    
    x = u[0]
    y = u[1]
    z = u[2]
    s = np.sin(theta)
    c = np.cos(theta)
    
    return np.array([[c + x**2 * (1 - c), x * y * (1 - c) - z * s, x * z * (1 - c) + y * s],
                     [y * x * (1 - c) + z * s, c + y**2 * (1 - c), y * z * (1 - c) - x * s ],
                     [z * x * (1 - c) - y * s, z * y * (1 - c) + x * s, c + z**2 * (1 - c) ]])
711533
public final void conjugate(Quaternion q1) {
		this.x = -q1.x;
		this.y = -q1.y;
		this.z = -q1.z;
		this.w = q1.w;
	}