Compute the gravitational accelerations in the earth-sun system.
    q in row vector of 6 elements: sun (x, y, z), earth (x, y, z)
509319
def accel_ES(q: np.ndarray):
    

    # Number of celestial bodies
    num_bodies: int = 2
    # Number of dimensions in arrays; 3 spatial dimensions times the number of bodies
    dims = 3 * num_bodies

    # Body 0 is the sun; Body 1 is the earth
    m0 = mass[0]
    m1 = mass[1]

    # Extract position of the sun and earth as 3-vectors
    pos_0 = q[slices[0]]
    pos_1 = q[slices[1]]

    # Displacement vector from sun to earth
    dv_01: np.ndarray = pos_1 - pos_0

    # Distance from sun to earth
    r_01: float = np.linalg.norm(dv_01)
    
    # Unit vector pointing from sun to earth
    udv_01 = dv_01 / r_01

    # The force between these has magnitude G*m0*m1 / r^2
    f_01: float = (G * m0 * m1) / (r_01 ** 2)
    
    # Initialize acceleration as 6x1 array
    a: np.ndarray = np.zeros(dims)
    
    # The force vectors are attractive
    a[slices[0]] += f_01 * udv_01 / m0
    a[slices[1]] -= f_01 * udv_01 / m1

    # Return the acceleration vector
    return a
107122
def plot_ES(q: np.ndarray, sim_name: str, fname: Optional[str] = None):
    
    # Convert all distances from meters to astronomical units (AU)
    # Unpack sun (x, y) in au; z not used in plots
    sun_x = q[:,0] / au2m
    sun_y = q[:,1] / au2m
    # Unpack earth (x, y); z not used in plots
    earth_x = q[:,3] / au2m
    earth_y = q[:,4] / au2m
    
    # Set up chart title and scale
    fig, ax = plt.subplots(figsize=[12,12])
    ax.set_title(f'Orbit of Earth in 2018; Weekly from {sim_name}')
    ax.set_xlabel('x in J2000.0 Frame; Astronomical Units (au)')
    ax.set_ylabel('y in J2000.0 Frame; Astronomical Units (au)')
    a = 1.2
    ticks = np.arange(-a, a+0.2, 0.2)
    ax.set_xlim(-a, a)
    ax.set_ylim(-a, a)
    ax.set_xticks(ticks)
    ax.set_yticks(ticks)

    # Set marker sizes proportional to size of bodies    
    radius_earth = radius_tbl['earth']
    markersize_earth = 8.0
    markersize_sun = cbrt(radius_tbl['sun'] / radius_earth) * markersize_earth
    
    # Orbit of the Sun (it moves a little in barycentric coordinates)
    ax.plot(sun_x, sun_y, label='Sun', color='orange', linewidth=0, markersize = markersize_sun, marker='o')
    # Orbit of the Earth
    ax.plot(earth_x, earth_y, label='Earth', color='b', linewidth=0, markersize = markersize_earth, marker='o')
    
    # Legend and grid
    ax.legend()
    ax.grid()

    # Save plot if a filename was provided
    if fname is not None:
        fig.savefig(fname, bbox_inches='tight')

    # Display plot
    plt.show()
657239
def xstep(self):
        
        """

        self.YU[:] = self.Y - self.U
        self.block_sep0(self.YU)[:] += self.S
        Zf = sl.rfftn(self.YU, None, self.cri.axisN)
        Z0f = self.block_sep0(Zf)
        Z1f = self.block_sep1(Zf)

        DZ0f = np.conj(self.Df) * Z0f
        DZ0fBQ = sl.dot(self.B.dot(self.Q).T, DZ0f, axis=self.cri.axisC)
        Z1fQ = sl.dot(self.Q.T, Z1f, axis=self.cri.axisC)
        b = DZ0fBQ + Z1fQ

        Xh = sl.solvedbd_sm(self.gDf, (self.mu / self.rho) * self.GHGf + 1.0,
                            b, self.c, axis=self.cri.axisM)
        self.Xf[:] = sl.dot(self.Q, Xh, axis=self.cri.axisC)
        self.X = sl.irfftn(self.Xf, self.cri.Nv, self.cri.axisN)

        if self.opt['LinSolveCheck']:
            DDXf = np.conj(self.Df) *  sl.inner(self.Df, self.Xf,
                                                axis=self.cri.axisM)
            DDXfBB = sl.dot(self.B.T.dot(self.B), DDXf, axis=self.cri.axisC)
            ax = self.rho * (DDXfBB + self.Xf) + \
                 self.mu * self.GHGf * self.Xf
            b = self.rho * (sl.dot(self.B.T, DZ0f, axis=self.cri.axisC)
                            + Z1f)
            self.xrrs = sl.rrs(ax, b)
        else:
            self.xrrs = None
1037299
function(w, x, y, z) {

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

      var eps = Quaternion['EPSILON'];

      // maybe check for NaN's here?
      return Math.abs(P['w'] - this['w']) < eps
              && Math.abs(P['x'] - this['x']) < eps
              && Math.abs(P['y'] - this['y']) < eps
              && Math.abs(P['z'] - this['z']) < eps;
    }
521694
def _new_population_gsa(population, fitnesses, velocities, lower_bounds,
                        upper_bounds, grav_initial, grav_reduction_rate,
                        iteration, max_iterations):
    
    # Update the gravitational constant, and the best and worst of the population
    # Calculate the mass and acceleration for each solution
    # Update the velocity and position of each solution
    population_size = len(population)
    solution_size = len(population[0])

    # In GSA paper, grav is G
    grav = _next_grav_gsa(grav_initial, grav_reduction_rate, iteration,
                          max_iterations)
    masses = _get_masses(fitnesses)

    # Create bundled solution with position and mass for the K best calculation
    # Also store index to later check if two solutions are the same
    # Sorted by solution fitness (mass)
    solutions = [{
        'pos': pos,
        'mass': mass,
        'index': i
    } for i, (pos, mass) in enumerate(zip(population, masses))]
    solutions.sort(key=lambda x: x['mass'], reverse=True)

    # Get the force on each solution
    # Only the best K solutions apply force
    # K linearly decreases to 1
    num_best = int(population_size - (population_size - 1) *
                   (iteration / float(max_iterations)))
    forces = []
    for i in range(population_size):
        force_vectors = []
        for j in range(num_best):
            # If it is not the same solution
            if i != solutions[j]['index']:
                force_vectors.append(
                    _gsa_force(grav, masses[i], solutions[j]['mass'],
                               population[i], solutions[j]['pos']))
        forces.append(_gsa_total_force(force_vectors, solution_size))

    # Get the acceleration of each solution
    accelerations = []
    for i in range(population_size):
        accelerations.append(_gsa_acceleration(forces[i], masses[i]))

    # Update the velocity of each solution
    new_velocities = []
    for i in range(population_size):
        new_velocities.append(
            _gsa_update_velocity(velocities[i], accelerations[i]))

    # Create the new population
    new_population = []
    for i in range(population_size):
        new_position = _gsa_update_position(population[i], new_velocities[i])
        # Constrain to bounds
        new_position = list(
            numpy.clip(new_position, lower_bounds, upper_bounds))

        new_population.append(new_position)

    return new_population, new_velocities
780517
def acceleration_of_satellite(position, mass):
    """"""
    F_x, F_y = force_on_satellite(position, mass)
    return F_x / mass, F_y / mass
276058
def get_forces(self, a):
        """"""
        f = np.zeros( [ len(a), 3 ], dtype=float )
        for c in self.calcs:
            f += c.get_forces(a)
        return f
1229141
def inertia(x, y, z, weight=None):
    """"""
    if weight is None:
        weight = 1
    tensor_of_inertia = np.zeros((3, 3), dtype=float)
    tensor_of_inertia[0][0] = (y * y + z * z) * weight
    tensor_of_inertia[0][1] = (-1) * x * y * weight
    tensor_of_inertia[0][2] = (-1) * x * z * weight
    tensor_of_inertia[1][0] = (-1) * x * y * weight
    tensor_of_inertia[1][1] = (x * x + z * z) * weight
    tensor_of_inertia[1][2] = (-1) * y * z * weight
    tensor_of_inertia[2][0] = (-1) * x * z * weight
    tensor_of_inertia[2][1] = (-1) * z * y * weight
    tensor_of_inertia[2][2] = (x * x + y * y) * weight

    eigen_values = np.linalg.eigvals(tensor_of_inertia)
    small_inertia = eigen_values[2][2]
    middle_inertia = eigen_values[1][1]
    big_inertia = eigen_values[0][0]
    return small_inertia, middle_inertia, big_inertia
450988
def load_constants():
    """"""
    
    # The universal gravitational constant
    # https://en.wikipedia.org/wiki/Gravitational_constant
    G: float = 6.67408E-11
    
    # The names of the celestial bodies
    body_name = \
        ['sun',
         'moon',
         'mercury',
         'venus',
         'earth',
         'mars',
         'jupiter',
         'saturn',
         'uranus',
         'neptune']
    
    # The mass of the celestial bodies
    # https://en.wikipedia.org/wiki/Earth_mass
    mass_earth: float = 5.9722E24
    mass: Dict[str, float]  = \
        {'sun': mass_earth * 332946.0487,
         'moon' : mass_earth * 0.012300037,
         'mercury': mass_earth * 0.0553,
         'venus': mass_earth * 0.815, 
         'earth': mass_earth * 1.0000,
         'mars': mass_earth * 0.107,
         'jupiter': mass_earth * 317.8,
         'saturn': mass_earth * 95.2,
         'uranus': mass_earth * 14.5,
         'neptune': mass_earth * 17.1,
         }
    
    # The radii of the celestial bodiea
    # https://nineplanets.org/data1.html
    radius: Dict[str, float] = \
    {'sun': 695000e3,
     'moon': 1738e3,
     'mercury': 2440e3,
     'venus': 6052e3,
     'earth': 6378e3,
     'mars': 3397e3,
     'jupiter': 71492e3,
     'saturn': 60268e3,
     'uranus': 35559e3,
     'neptune': 24766e3     
     }

    return G, body_name, mass, radius
1048977
def cart2spher(x, y, z):
    """"""
    hxy = np.hypot(x, y)
    r = np.hypot(hxy, z)
    theta = np.arctan2(z, hxy)
    phi = np.arctan2(y, x)
    return r, theta, phi
660670
def quat2Yaw(qw, qx, qy, qz):
    '''
    

    '''
    rotateZa0=2.0*(qx*qy + qw*qz)
    rotateZa1=qw*qw + qx*qx - qy*qy - qz*qz
    rotateZ=0.0
    if(rotateZa0 != 0.0 and rotateZa1 != 0.0):
        rotateZ=atan2(rotateZa0,rotateZa1)
    return rotateZ
157407
public Quaternion fromAngles (float x, float y, float z) {
        // TODO: it may be more convenient to define the angles in the opposite order (first z,
        // then y, then x)
        float hx = x * 0.5f, hy = y * 0.5f, hz = z * 0.5f;
        float sz = FloatMath.sin(hz), cz = FloatMath.cos(hz);
        float sy = FloatMath.sin(hy), cy = FloatMath.cos(hy);
        float sx = FloatMath.sin(hx), cx = FloatMath.cos(hx);
        float szsy = sz*sy, czsy = cz*sy, szcy = sz*cy, czcy = cz*cy;
        return set(
            czcy*sx - szsy*cx,
            czsy*cx + szcy*sx,
            szcy*cx - czsy*sx,
            czcy*cx + szsy*sx);
    }
254321
function Quat2Angle(x, y, z, w) {
  const test = x * y + z * w;

  // singularity at north pole
  if (test > 0.499) {
    const yaw = 2 * Math.atan2(x, w);
    const pitch = Math.PI / 2;
    const roll = 0;

    return new THREE.Vector3(pitch, roll, yaw);
  }

  // singularity at south pole
  if (test < -0.499) {
    const yaw = -2 * Math.atan2(x, w);
    const pitch = -Math.PI / 2;
    const roll = 0;

    return new THREE.Vector3(pitch, roll, yaw);
  }

  const sqx = x * x;
  const sqy = y * y;
  const sqz = z * z;
  const yaw = Math.atan2(2 * y * w - 2 * x * z, 1 - 2 * sqy - 2 * sqz);
  const pitch = Math.asin(2 * test);
  const roll = Math.atan2(2 * x * w - 2 * y * z, 1 - 2 * sqx - 2 * sqz);

  return new THREE.Vector3(pitch, roll, yaw);
}
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,
    )
485694
public static void computePositions(Node node, int depth, double aoff, double awid, double radius, double radiusinc) {
    node.x = FastMath.sin(aoff) * radius;
    node.y = FastMath.cos(aoff) * radius;

    // Angular width per weight
    double cwid = awid / node.weight;
    // Avoid overly wide angles - shrink radius if necessary.
    double div = 1;
    if(node.weight > 1) {
      double s = FastMath.sin(awid * .5), c = FastMath.cos(awid * .5);
      double dx = s * (depth + 1), dy = c * (depth + 1) - depth;
      double d = FastMath.sqrt(dx * dx + dy * dy) / MathUtil.SQRT2;
      div = Math.max(div, d);
    }
    // Angular position of current child:
    double cang = aoff - awid * .5 / div;
    final double adjwid = cwid / div;
    for(Node c : node.children) {
      computePositions(c, depth + 1, cang + adjwid * c.weight * .5, adjwid * c.weight, radius + radiusinc, radiusinc);
      cang += adjwid * c.weight;
    }
  }
341573
function _getEulerAngles(q) {
  var poleSum = q.x * q.w - q.y * q.z;
  if (util.doublesEqual(poleSum, 0.5))
    return {"x": 90, "y": 0, "z": 0};
  else if (util.doublesEqual(poleSum, -0.5))
    return {"x": -90, "y": 0, "z": 0};

  var sqw = q.w * q.w;
  var sqx = q.x * q.x;
  var sqy = q.y * q.y;
  var sqz = q.z * q.z;

  //var _x = Math.atan2(2 * q.x * q.w - 2 * q.y * q.z, 1 - 2 * sqx  - 2 * sqz);
  var _x = Math.asin(2 * q.x * q.w - 2 * q.y * q.z)
  //var _y = Math.atan2(2 * q.y * q.w - 2 * q.x * q.z, 1 - 2 * sqy  - 2 * sqz);
  var _y = Math.atan2(2 * q.x * q.z + 2 * q.y * q.w, 1 - 2 * sqy - 2 * sqx);
  //var _z = Math.asin(2 * q.x * q.y + 2 * q.z * q.w);
  var _z = Math.PI - Math.atan2(2 * q.x * q.y + 2 * q.z * q.w, 1 - 2 * sqy - 2 * sqw);

  return {
    "x": _normalizeRad(_x),
    "y": _normalizeRad(_y),
    "z": _normalizeRad(_z)
  };
}
901502
def qrotate(vector, axis, angle):
    
    n_axis = axis / vnorm(axis)
    sin_angle = np.expand_dims(np.sin(angle / 2), 0)
    if np.ndim(n_axis) == 1:
        n_axis = np.expand_dims(n_axis, 1)
        p__ = np.dot(n_axis, sin_angle)[:, np.newaxis]
    else:
        p__ = n_axis * sin_angle

    q__ = Quaternion(np.cos(angle / 2), p__)
    shape = vector.shape
    return np.einsum("kj, ikj->ij",
                     vector.reshape((3, -1)),
                     q__.rotation_matrix()[:3, :3]).reshape(shape)