Returns a unit quaternion that will align vec with the z-axis
183666
def alignVec_quat(vec):
    """"""
    alpha = np.arctan2(vec[1], vec[0])
    beta = np.arccos(vec[2])
    gamma = -alpha*vec[2]
    cb = np.cos(0.5*beta)
    sb = np.sin(0.5*beta)
    return np.array([cb*np.cos(0.5*(alpha + gamma)),
                     sb*np.sin(0.5*(gamma - alpha)),
                     sb*np.cos(0.5*(gamma - alpha)),
                     cb*np.sin(0.5*(alpha + gamma))])
520485
def quat_multiply(quaternion1, quaternion0):
    
    x0, y0, z0, w0 = quaternion0
    x1, y1, z1, w1 = quaternion1
    return np.array(
        (
            x1 * w0 + y1 * z0 - z1 * y0 + w1 * x0,
            -x1 * z0 + y1 * w0 + z1 * x0 + w1 * y0,
            x1 * y0 - y1 * x0 + z1 * w0 + w1 * z0,
            -x1 * x0 - y1 * y0 - z1 * z0 + w1 * w0,
        ),
        dtype=np.float32,
    )
299428
public Quaternionf div(Quaternionfc b, Quaternionf dest) {
        float invNorm = 1.0f / (b.x() * b.x() + b.y() * b.y() + b.z() * b.z() + b.w() * b.w());
        float x = -b.x() * invNorm;
        float y = -b.y() * invNorm;
        float z = -b.z() * invNorm;
        float 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;
    }
492280
public Quaternionf ConvertDirectionalVectorToQuaternion(Vector3f d) {
        d.negate();
        Quaternionf q = new Quaternionf();
        // check for exception condition
        if ((d.x == 0) && (d.z == 0)) {
            // exception condition if direction is (0,y,0):
            // straight up, straight down or all zero's.
            if (d.y > 0) { // direction straight up
                AxisAngle4f angleAxis = new AxisAngle4f(-(float) Math.PI / 2, 1, 0,
                        0);
                q.set(angleAxis);
            } else if (d.y < 0) { // direction straight down
                AxisAngle4f angleAxis = new AxisAngle4f((float) Math.PI / 2, 1, 0, 0);
                q.set(angleAxis);
            } else { // All zero's. Just set to identity quaternion
                q.identity();
            }
        } else {
            d.normalize();
            Vector3f up = new Vector3f(0, 1, 0);
            Vector3f s = new Vector3f();
            d.cross(up, s);
            s.normalize();
            Vector3f u = new Vector3f();
            d.cross(s, u);
            u.normalize();
            Matrix4f matrix = new Matrix4f(s.x, s.y, s.z, 0, u.x, u.y, u.z, 0, d.x,
                    d.y, d.z, 0, 0, 0, 0, 1);
            q.setFromNormalized(matrix);
        }
        return q;
    }
229089
def quaternion_conjugate(quaternion):
    
    return numpy.array((-quaternion[0], -quaternion[1],
                        -quaternion[2], quaternion[3]), dtype=numpy.float64)
364405
def vectorAngle(vec1,vec2):
    '''
    
    '''
    if vec1.ndim == 1:
        assert vec2.ndim == 3
        vec1 = vectorToField(vec1, vec2.shape[:2])

    if vec2.ndim == 1:
        assert vec1.ndim == 3
        vec2 = vectorToField(vec2, vec1.shape[1:])
    a = np.arccos(
                np.einsum('ijk,ijk->jk', vec1,vec2)
                /( norm(vec1,axis=0) * norm(vec2,axis=0) ) )
    #take smaller of both possible angles:
    ab = np.abs(np.pi-a)
    with np.errstate(invalid='ignore'):
        i = a>ab
    a[i] = ab[i]
    return a
512907
def to_quaternion(roll = 0.0, pitch = 0.0, yaw = 0.0):
    
    t0 = math.cos(math.radians(yaw * 0.5))
    t1 = math.sin(math.radians(yaw * 0.5))
    t2 = math.cos(math.radians(roll * 0.5))
    t3 = math.sin(math.radians(roll * 0.5))
    t4 = math.cos(math.radians(pitch * 0.5))
    t5 = math.sin(math.radians(pitch * 0.5))

    w = t0 * t2 * t4 + t1 * t3 * t5
    x = t0 * t3 * t4 - t1 * t2 * t5
    y = t0 * t2 * t5 + t1 * t3 * t4
    z = t1 * t2 * t4 - t0 * t3 * t5

    return [w, x, y, z]
111666
def unit_vector(self):
        """"""
        x = -math.cos(self.rpitch) * math.sin(self.ryaw)
        y = -math.sin(self.rpitch)
        z = math.cos(self.rpitch) * math.cos(self.ryaw)
        return Vector3(x, y, z)
577702
def log(self):
        
        
        # Init
        norm = self.norm()
        vecNorm = self.x**2 + self.y**2 + self.z**2
        tmp = self.w / norm
        q = Quaternion()
        
        # Calculate
        q.w = np.log(norm)
        q.x = np.log(norm) * self.x * np.arccos(tmp) / vecNorm
        q.y = np.log(norm) * self.y * np.arccos(tmp) / vecNorm
        q.z = np.log(norm) * self.z * np.arccos(tmp) / vecNorm
        
        return q
781144
function(w, x, y, z) {

      parse(P, w, x, y, z);

      if (P['y'] === 0 && P['z'] === 0) {

        if (P['w'] === 1 && P['x'] === 0) {
          return this;
        }

        if (P['w'] === 0 && P['x'] === 0) {
          return Quaternion['ONE'];
        }

        // Check if we can operate in C
        // Borrowed from complex.js
        if (this['y'] === 0 && this['z'] === 0) {

          var a = this['w'];
          var b = this['x'];

          if (a === 0 && b === 0) {
            return Quaternion['ZERO'];
          }

          var arg = Math.atan2(b, a);
          var loh = logHypot(a, b);

          if (P['x'] === 0) {

            if (b === 0 && a >= 0) {

              return new Quaternion(Math.pow(a, P['w']), 0, 0, 0);

            } else if (a === 0) {

              switch (P['w'] % 4) {
                case 0:
                  return new Quaternion(Math.pow(b, P['w']), 0, 0, 0);
                case 1:
                  return new Quaternion(0, Math.pow(b, P['w']), 0, 0);
                case 2:
                  return new Quaternion(-Math.pow(b, P['w']), 0, 0, 0);
                case 3:
                  return new Quaternion(0, -Math.pow(b, P['w']), 0, 0);
              }
            }
          }

          a = Math.exp(P['w'] * loh - P['x'] * arg);
          b = P['x'] * loh + P['w'] * arg;
          return new Quaternion(
                  a * Math.cos(b),
                  a * Math.sin(b), 0, 0);
        }
      }

      // Normal quaternion behavior
      // q^p = e^ln(q^p) = e^(ln(q)*p)
      return this.log().mul(P).exp();
    }
1291862
def vec2pix(nside, x, y, z, nest=False):
    """"""
    theta, phi = vec2ang(np.transpose([x, y, z]))
    # hp.vec2ang() returns raveled arrays, which are 1D.
    if np.isscalar(x):
        theta = theta.item()
        phi = phi.item()
    else:
        shape = np.shape(x)
        theta = theta.reshape(shape)
        phi = phi.reshape(shape)
    lon, lat = _healpy_to_lonlat(theta, phi)
    return lonlat_to_healpix(lon, lat, nside, order='nested' if nest else 'ring')
11573
public Quaterniond integrate(double dt, double vx, double vy, double vz, Quaterniond dest) {
        double thetaX = dt * vx * 0.5;
        double thetaY = dt * vy * 0.5;
        double thetaZ = dt * vz * 0.5;
        double thetaMagSq = thetaX * thetaX + thetaY * thetaY + thetaZ * thetaZ;
        double s;
        double dqX, dqY, dqZ, dqW;
        if (thetaMagSq * thetaMagSq / 24.0 < 1E-8) {
            dqW = 1.0 - thetaMagSq * 0.5;
            s = 1.0 - thetaMagSq / 6.0;
        } else {
            double thetaMag = Math.sqrt(thetaMagSq);
            double sin = Math.sin(thetaMag);
            s = sin / thetaMag;
            dqW = Math.cosFromSin(sin, thetaMag);
        }
        dqX = thetaX * s;
        dqY = thetaY * s;
        dqZ = thetaZ * s;
        /* Pre-multiplication */
        dest.set(dqW * x + dqX * w + dqY * z - dqZ * y,
                 dqW * y - dqX * z + dqY * w + dqZ * x,
                 dqW * z + dqX * y - dqY * x + dqZ * w,
                 dqW * w - dqX * x - dqY * y - dqZ * z);
        return dest;
    }
1111140
def unit(self):
        """"""
        return Vector(
            (self.x / self.magnitude()),
            (self.y / self.magnitude()),
            (self.z / self.magnitude())
        )
117196
def from_axis_angle(axis, angle):
    

    sincomp = math.sin(angle / 2.0)
    return Quaternion(
      axis.x * sincomp, axis.y * sincomp,
      axis.z * sincomp, math.cos(angle / 2.0))
75877
public Quaternion fromVectorFromNegativeZ (double tx, double ty, double tz) {
        double angle = Math.acos(-tz);
        if (angle < MathUtil.EPSILON) {
            return set(IDENTITY);
        }
        if (angle > Math.PI - MathUtil.EPSILON) {
            return set(0f, 1f, 0f, 0f); // 180 degrees about y
        }
        double len = Math.hypot(tx, ty);
        return fromAngleAxis(angle, ty/len, -tx/len, 0f);
    }
406698
public Quaternion fromVectors (IVector3 from, IVector3 to) {
        float angle = from.angle(to);
        if (angle < MathUtil.EPSILON) {
            return set(IDENTITY);
        }
        if (angle <= FloatMath.PI - MathUtil.EPSILON) {
            return fromAngleAxis(angle, from.cross(to).normalizeLocal());
        }
        // it's a 180 degree rotation; any axis orthogonal to the from vector will do
        Vector3 axis = new Vector3(0f, from.z(), -from.y());
        float length = axis.length();
        return fromAngleAxis(FloatMath.PI, length < MathUtil.EPSILON ?
                             axis.set(-from.z(), 0f, from.x()).normalizeLocal() :
                             axis.multLocal(1f / length));
    }
591320
def transform_vector_coorb_to_inertial(vec_coorb, orbPhase, quat_copr):
    

    # Transform to coprecessing frame
    vec_copr = rotate_in_plane(vec_coorb, -orbPhase)

    # Transform to inertial frame
    vec = transformTimeDependentVector(np.array([quat_copr]).T,
        np.array([vec_copr]).T).T[0]

    return np.array(vec)