diff --git a/src/config_loader.cpp b/src/config_loader.cpp
index cb7a9d5..749fe17 100644
--- a/src/config_loader.cpp
+++ b/src/config_loader.cpp
@@ -91,14 +91,6 @@ bool parse_toml_body(toml_datum_t body_table, CelestialBody* body) {
     return true;
 }
 
-struct OrbitParams {
-    double eccentricity;
-    double semi_major_axis;
-};
-
-// Forward declaration
-void calculate_velocities(SimulationState* sim, OrbitParams* orbit_params);
-
 // Load system configuration from TOML file
 bool load_system_config(SimulationState* sim, const char* filepath) {
     toml_result_t result = toml_parse_file_ex(filepath);
@@ -131,9 +123,6 @@ bool load_system_config(SimulationState* sim, const char* filepath) {
         return false;
     }
 
-    // Parse each body and store orbit params
-    OrbitParams orbit_params[100];
-
     for (int i = 0; i < body_count; i++) {
         toml_datum_t body_table = bodies.u.arr.elem[i];
         if (!parse_toml_body(body_table, &sim->bodies[i])) {
@@ -141,189 +130,14 @@ bool load_system_config(SimulationState* sim, const char* filepath) {
             toml_free(result);
             return false;
         }
-
-        // Store orbit parameters for velocity calculation
-        orbit_params[i].eccentricity = sim->bodies[i].eccentricity;
-        orbit_params[i].semi_major_axis = sim->bodies[i].semi_major_axis;
     }
 
     sim->body_count = body_count;
     toml_free(result);
 
-    // Calculate initial velocities
-    calculate_velocities(sim, orbit_params);
-
-    // Calculate SOI radii
+    calculate_initial_velocities(sim);
     calculate_soi_radii(sim);
 
     printf("Loaded %d bodies from %s\n", body_count, filepath);
     return true;
 }
-
-// Calculate initial velocities using vis-viva equation
-void calculate_velocities(SimulationState* sim, OrbitParams* orbit_params) {
-    // First, handle multiple root bodies (binary stars, etc.)
-    // Find all root bodies and calculate barycentric orbits
-    int root_count = 0;
-    int root_indices[32];  // Max 32 root bodies
-    Vec3 barycenter = {0.0, 0.0, 0.0};
-    double total_mass = 0.0;
-
-    // Find all root bodies and calculate barycenter
-    for (int i = 0; i < sim->body_count; i++) {
-        if (sim->bodies[i].parent_index == -1) {
-            if (root_count < 32) {
-                root_indices[root_count++] = i;
-                // Weighted sum for barycenter
-                Vec3 weighted_pos = vec3_scale(sim->bodies[i].position, sim->bodies[i].mass);
-                barycenter = vec3_add(barycenter, weighted_pos);
-                total_mass += sim->bodies[i].mass;
-            }
-        }
-    }
-
-    // Calculate barycenter position
-    if (total_mass > 0.0) {
-        barycenter = vec3_scale(barycenter, 1.0 / total_mass);
-    }
-
-    // Debug output for multiple root bodies
-    if (root_count > 1) {
-        printf("\nBinary/Multiple star system detected:\n");
-        printf("  Number of root bodies: %d\n", root_count);
-        printf("  Barycenter position: (%.3e, %.3e, %.3e) m\n",
-               barycenter.x, barycenter.y, barycenter.z);
-        printf("  Total system mass: %.3e kg\n", total_mass);
-    }
-
-    // Set velocities for root bodies to orbit barycenter
-    if (root_count > 1) {
-        for (int i = 0; i < root_count; i++) {
-            CelestialBody* body = &sim->bodies[root_indices[i]];
-
-            // Calculate position relative to barycenter
-            Vec3 r = vec3_sub(body->position, barycenter);
-            double distance = vec3_magnitude(r);
-
-            if (distance < 1.0) {
-                body->velocity = {0.0, 0.0, 0.0};
-                continue;
-            }
-
-            // Calculate total mass of OTHER root bodies
-            double other_mass = total_mass - body->mass;
-
-            // Calculate circular orbit speed around barycenter
-            // v = sqrt(G * M_other / r)
-            double speed = sqrt(G * other_mass / distance);
-
-            // Create velocity perpendicular to position vector
-            Vec3 z_axis = {0.0, 0.0, 1.0};
-            Vec3 vel_dir = {
-                r.y * z_axis.z - r.z * z_axis.y,
-                r.z * z_axis.x - r.x * z_axis.z,
-                r.x * z_axis.y - r.y * z_axis.x
-            };
-
-            // If r is parallel to z-axis, use x-axis instead
-            double cross_mag = vec3_magnitude(vel_dir);
-            if (cross_mag < 0.01) {
-                Vec3 x_axis = {1.0, 0.0, 0.0};
-                vel_dir.x = r.y * x_axis.z - r.z * x_axis.y;
-                vel_dir.y = r.z * x_axis.x - r.x * x_axis.z;
-                vel_dir.z = r.x * x_axis.y - r.y * x_axis.x;
-            }
-
-            // Normalize and scale by orbital speed
-            vel_dir = vec3_normalize(vel_dir);
-            body->velocity = vec3_scale(vel_dir, speed);
-
-            // Debug output
-            printf("  %s: distance from barycenter = %.3e m, orbital speed = %.3e m/s\n",
-                   body->name, distance, speed);
-        }
-    } else if (root_count == 1) {
-        // Single root body stays stationary
-        sim->bodies[root_indices[0]].velocity = {0.0, 0.0, 0.0};
-    }
-
-    // Now handle child bodies (planets, moons, etc.)
-    for (int i = 0; i < sim->body_count; i++) {
-        CelestialBody* body = &sim->bodies[i];
-
-        // Skip root bodies (already handled)
-        if (body->parent_index == -1) {
-            continue;
-        }
-
-        // Get parent body
-        if (body->parent_index >= 0 && body->parent_index < sim->body_count) {
-            CelestialBody* parent = &sim->bodies[body->parent_index];
-
-            // Calculate relative position
-            Vec3 r = vec3_sub(body->position, parent->position);
-            double distance = vec3_magnitude(r);
-
-            if (distance < 1.0) {
-                body->velocity = {0.0, 0.0, 0.0};
-                continue;
-            }
-
-            double e = orbit_params[i].eccentricity;
-            double a = orbit_params[i].semi_major_axis;
-
-            // Use vis-viva equation: v = sqrt(G*M*(2/r - 1/a))
-            double speed = sqrt(G * parent->mass * (2.0 / distance - 1.0 / a));
-
-            if (e > 0.01) {
-                printf("  %s: eccentric orbit (e=%.2f, a=%.3e m), speed at r=%.3e m: %.3f km/s\n",
-                       body->name, e, a, distance, speed / 1000.0);
-            }
-
-            // Create velocity perpendicular to position vector
-            // If position is mostly in XY plane, make velocity in XY plane
-            // Cross product of r with z-axis gives perpendicular vector in XY plane
-            Vec3 z_axis = {0.0, 0.0, 1.0};
-
-            // Calculate cross product: r x z_axis
-            Vec3 vel_dir = {
-                r.y * z_axis.z - r.z * z_axis.y,
-                r.z * z_axis.x - r.x * z_axis.z,
-                r.x * z_axis.y - r.y * z_axis.x
-            };
-
-            // If r is parallel to z-axis, use x-axis instead
-            double cross_mag = vec3_magnitude(vel_dir);
-            if (cross_mag < 0.01) {
-                Vec3 x_axis = {1.0, 0.0, 0.0};
-                vel_dir.x = r.y * x_axis.z - r.z * x_axis.y;
-                vel_dir.y = r.z * x_axis.x - r.x * x_axis.z;
-                vel_dir.z = r.x * x_axis.y - r.y * x_axis.x;
-            }
-
-            // Normalize and scale by orbital speed
-            vel_dir = vec3_normalize(vel_dir);
-            body->velocity = vec3_scale(vel_dir, speed);
-
-            // Add parent's velocity for absolute reference frame
-            body->velocity = vec3_add(body->velocity, parent->velocity);
-        }
-    }
-}
-
-// Calculate SOI radii for all bodies
-void calculate_soi_radii(SimulationState* sim) {
-    for (int i = 0; i < sim->body_count; i++) {
-        CelestialBody* body = &sim->bodies[i];
-
-        if (body->parent_index == -1) {
-            // Root body has very large SOI
-            body->soi_radius = 1e15;  // ~1000 AU
-        } else if (body->parent_index >= 0 && body->parent_index < sim->body_count) {
-            CelestialBody* parent = &sim->bodies[body->parent_index];
-
-            // Update SOI using Hill sphere approximation
-            update_soi(body, parent, body->semi_major_axis);
-        }
-    }
-}
diff --git a/src/config_loader.h b/src/config_loader.h
index 42c3542..ffbb3e0 100644
--- a/src/config_loader.h
+++ b/src/config_loader.h
@@ -12,10 +12,4 @@ bool extract_vec3_from_table(toml_datum_t table, Vec3* pos);
 bool extract_color_from_table(toml_datum_t table, float* color);
 bool parse_toml_body(toml_datum_t body_table, CelestialBody* body);
 
-// Calculate initial circular orbit velocities for all bodies
-void calculate_initial_velocities(SimulationState* sim);
-
-// Calculate SOI radii for all bodies
-void calculate_soi_radii(SimulationState* sim);
-
 #endif
diff --git a/src/physics.cpp b/src/physics.cpp
index fe32d23..0fd45e1 100644
--- a/src/physics.cpp
+++ b/src/physics.cpp
@@ -12,6 +12,15 @@ Vec3 vec3_sub(Vec3 a, Vec3 b) {
     return {a.x - b.x, a.y - b.y, a.z - b.z};
 }
 
+// Cross product
+Vec3 vec3_cross(Vec3 a, Vec3 b) {
+    return {
+        a.y * b.z - a.z * b.y,
+        a.z * b.x - a.x * b.z,
+        a.x * b.y - a.y * b.x
+    };
+}
+
 // Scalar multiplication
 Vec3 vec3_scale(Vec3 v, double s) {
     return {v.x * s, v.y * s, v.z * s};
diff --git a/src/physics.h b/src/physics.h
index 2656917..eb8ecc9 100644
--- a/src/physics.h
+++ b/src/physics.h
@@ -15,6 +15,7 @@ const double G = 6.67430e-11;
 // Vector math functions
 Vec3 vec3_add(Vec3 a, Vec3 b);
 Vec3 vec3_sub(Vec3 a, Vec3 b);
+Vec3 vec3_cross(Vec3 a, Vec3 b);
 Vec3 vec3_scale(Vec3 v, double s);
 double vec3_magnitude(Vec3 v);
 double vec3_distance(Vec3 a, Vec3 b);
diff --git a/src/simulation.cpp b/src/simulation.cpp
index 9ec652e..63bbff1 100644
--- a/src/simulation.cpp
+++ b/src/simulation.cpp
@@ -139,6 +139,170 @@ void update_simulation(SimulationState* sim) {
     sim->time += sim->dt;
 }
 
+static void compute_perpendicular_orbital_velocity(CelestialBody* body, Vec3 center,
+                                                   double orbiting_mass) {
+    Vec3 r = vec3_sub(body->position, center);
+    double distance = vec3_magnitude(r);
+
+    if (distance < 1.0) {
+        body->velocity = {0.0, 0.0, 0.0};
+        return;
+    }
+
+    double speed = sqrt(G * orbiting_mass / distance);
+    Vec3 z_axis = {0.0, 0.0, 1.0};
+    Vec3 vel_dir = vec3_cross(r, z_axis);
+
+    if (vec3_magnitude(vel_dir) < 0.01) {
+        Vec3 x_axis = {1.0, 0.0, 0.0};
+        vel_dir = vec3_cross(r, x_axis);
+    }
+
+    vel_dir = vec3_normalize(vel_dir);
+    body->velocity = vec3_scale(vel_dir, speed);
+}
+
+static void compute_orbital_velocity_from_vis_viva(CelestialBody* body,
+                                                     CelestialBody* parent) {
+    Vec3 r = vec3_sub(body->position, parent->position);
+    double distance = vec3_magnitude(r);
+
+    if (distance < 1.0) {
+        body->velocity = {0.0, 0.0, 0.0};
+        return;
+    }
+
+    double e = body->eccentricity;
+    double a = body->semi_major_axis;
+    double speed = sqrt(G * parent->mass * (2.0 / distance - 1.0 / a));
+
+    if (e > 0.01) {
+        printf("  %s: eccentric orbit (e=%.2f, a=%.3e m), speed at r=%.3e m: %.3f km/s\n",
+               body->name, e, a, distance, speed / 1000.0);
+    }
+
+    Vec3 z_axis = {0.0, 0.0, 1.0};
+    Vec3 vel_dir = vec3_cross(r, z_axis);
+
+    if (vec3_magnitude(vel_dir) < 0.01) {
+        Vec3 x_axis = {1.0, 0.0, 0.0};
+        vel_dir = vec3_cross(r, x_axis);
+    }
+
+    vel_dir = vec3_normalize(vel_dir);
+    body->velocity = vec3_scale(vel_dir, speed);
+    body->velocity = vec3_add(body->velocity, parent->velocity);
+}
+
+static Vec3 compute_system_barycenter(SimulationState* sim, int* root_indices,
+                                      int* root_count) {
+    Vec3 barycenter = {0.0, 0.0, 0.0};
+    *root_count = 0;
+
+    for (int i = 0; i < sim->body_count; i++) {
+        if (sim->bodies[i].parent_index == -1) {
+            if (*root_count < 32) {
+                root_indices[(*root_count)++] = i;
+                Vec3 weighted_pos = vec3_scale(sim->bodies[i].position, sim->bodies[i].mass);
+                barycenter = vec3_add(barycenter, weighted_pos);
+            }
+        }
+    }
+
+    double total_mass = 0.0;
+    for (int i = 0; i < *root_count; i++) {
+        total_mass += sim->bodies[root_indices[i]].mass;
+    }
+
+    if (total_mass > 0.0) {
+        barycenter = vec3_scale(barycenter, 1.0 / total_mass);
+    }
+
+    return barycenter;
+}
+
+static double compute_total_root_mass(SimulationState* sim, int* root_indices,
+                                      int root_count) {
+    double total_mass = 0.0;
+    for (int i = 0; i < root_count; i++) {
+        total_mass += sim->bodies[root_indices[i]].mass;
+    }
+    return total_mass;
+}
+
+static void set_root_bodies_velocity(SimulationState* sim, int* root_indices,
+                                     int root_count, Vec3 barycenter, double total_mass) {
+    for (int i = 0; i < root_count; i++) {
+        CelestialBody* body = &sim->bodies[root_indices[i]];
+        double other_mass = total_mass - body->mass;
+        compute_perpendicular_orbital_velocity(body, barycenter, other_mass);
+
+        double distance = vec3_magnitude(vec3_sub(body->position, barycenter));
+        double speed = vec3_magnitude(body->velocity);
+        printf("  %s: distance from barycenter = %.3e m, orbital speed = %.3e m/s\n",
+               body->name, distance, speed);
+    }
+}
+
+static void set_child_bodies_velocity(SimulationState* sim) {
+    for (int i = 0; i < sim->body_count; i++) {
+        CelestialBody* body = &sim->bodies[i];
+
+        if (body->parent_index == -1) {
+            continue;
+        }
+
+        if (body->parent_index >= 0 && body->parent_index < sim->body_count) {
+            CelestialBody* parent = &sim->bodies[body->parent_index];
+            compute_orbital_velocity_from_vis_viva(body, parent);
+        }
+    }
+}
+
+static void print_system_info_if_multiple_roots(int root_count, Vec3 barycenter) {
+    if (root_count > 1) {
+        printf("\nBinary/Multiple star system detected:\n");
+        printf("  Number of root bodies: %d\n", root_count);
+        printf("  Barycenter position: (%.3e, %.3e, %.3e) m\n",
+               barycenter.x, barycenter.y, barycenter.z);
+    }
+}
+
+void compute_initial_velocities(SimulationState* sim) {
+    int root_indices[32];
+    int root_count;
+    Vec3 barycenter = compute_system_barycenter(sim, root_indices, &root_count);
+
+    print_system_info_if_multiple_roots(root_count, barycenter);
+
+    if (root_count > 1) {
+        double total_mass = compute_total_root_mass(sim, root_indices, root_count);
+        set_root_bodies_velocity(sim, root_indices, root_count, barycenter, total_mass);
+    } else if (root_count == 1) {
+        sim->bodies[root_indices[0]].velocity = {0.0, 0.0, 0.0};
+    }
+
+    set_child_bodies_velocity(sim);
+}
+
+void calculate_initial_velocities(SimulationState* sim) {
+    compute_initial_velocities(sim);
+}
+
+// Calculate SOI radii for all bodies
+void calculate_soi_radii(SimulationState* sim) {
+    for (int i = 0; i < sim->body_count; i++) {
+        CelestialBody* body = &sim->bodies[i];
+
+        if (body->parent_index == -1) {
+            body->soi_radius = 1e15;
+        } else if (body->parent_index >= 0 && body->parent_index < sim->body_count) {
+            CelestialBody* parent = &sim->bodies[body->parent_index];
+            update_soi(body, parent, body->semi_major_axis);
+        }
+    }
+}
+
 OrbitalElements calculate_orbital_elements(CelestialBody* body, CelestialBody* primary,
                                            CelestialBody* optional_ref_body, double current_time) {
     const double AU = 1.496e11;
diff --git a/src/simulation.h b/src/simulation.h
index d97e83a..739680f 100644
--- a/src/simulation.h
+++ b/src/simulation.h
@@ -38,6 +38,13 @@ int find_dominant_body(SimulationState* sim, int body_index);
 void update_soi(CelestialBody* body, CelestialBody* parent, double semi_major_axis);
 void update_simulation(SimulationState* sim);
 
+// Velocity initialization
+void compute_initial_velocities(SimulationState* sim);
+void calculate_initial_velocities(SimulationState* sim);
+
+// SOI helpers
+void calculate_soi_radii(SimulationState* sim);
+
 // Orbital elements calculation
 struct OrbitalElements {
     double time_days;