diff --git a/graphics/center_of_mass.jl b/graphics/center_of_mass.jl
index 79131b2..448b492 100644
--- a/graphics/center_of_mass.jl
+++ b/graphics/center_of_mass.jl
@@ -1,17 +1,6 @@
 using Luxor
 
-using ReCo: restrict_coordinate
-
-##
-
-function project(x, L)
-    φ = (x + L) * π / L
-    return Point(cos(φ), -sin(φ))
-end
-
-function project_back(θ, L)
-    return θ * L / π - L
-end
+using ReCo: restrict_coordinate, project_to_unit_circle, project_back_from_unit_circle
 
 ##
 
@@ -26,14 +15,20 @@ A = -0.9 * L
 B = 0.65 * L
 pr = 0.03 * L
 
-Ap = project(A, L)
-Bp = project(B, L)
+Ap_array = project_to_unit_circle(A, L)
+Bp_array = project_to_unit_circle(B, L)
+for p in (Ap_array, Bp_array)
+    p[2] *= -1
+end
+
+Ap = Point(Ap_array)
+Bp = Point(Bp_array)
 
 M = R * ((Ap + Bp) / 2)
 
 θ = atan(-M[2], M[1])
 
-COM = restrict_coordinate(project_back(θ, L); l=L)
+COM = project_back_from_unit_circle(θ, L)
 COMp = R * Point(cos(θ), -sin(θ))
 
 ##
diff --git a/src/ReCo.jl b/src/ReCo.jl
index e27d1a5..cff1ce0 100644
--- a/src/ReCo.jl
+++ b/src/ReCo.jl
@@ -7,6 +7,7 @@ include("Particle.jl")
 include("data.jl")
 include("setup.jl")
 include("simulation.jl")
+include("shape.jl")
 include("run.jl")
 
 end # module
\ No newline at end of file
diff --git a/src/shape.jl b/src/shape.jl
new file mode 100644
index 0000000..fbfc09b
--- /dev/null
+++ b/src/shape.jl
@@ -0,0 +1,45 @@
+using StaticArrays: SVector
+
+function project_to_unit_circle(x, l)
+    φ = (x + l) * π / l
+    return SVector(cos(φ), sin(φ))
+end
+
+function project_back_from_unit_circle(θ, l)
+    x = θ * l / π - l
+    return restrict_coordinate(x; l=l)
+end
+
+function center_of_mass(args)
+    x_proj_sum = SVector(0, 0)
+    y_proj_sum = SVector(0, 0)
+
+    for p in args.particles
+        x_proj_sum += project_to_unit_circle(p.c[1], args.l)
+        y_proj_sum += project_to_unit_circle(p.c[2], args.l)
+    end
+
+    # Prevent for example atan(1e-16, 1e-15) != 0 with rounding
+    digits = 5
+
+    # No need for 1/N with atan
+    # If proj is (0, 0) then COM is 0 or L or -L. Here, 0 is choosen with θ = π
+    if (round(x_proj_sum[1]; digits=digits) == 0) &&
+        (round(x_proj_sum[2]; digits=digits) == 0)
+        x_θ = π
+    else
+        x_θ = atan(x_proj_sum[2], x_proj_sum[1])
+    end
+
+    if (round(y_proj_sum[1]; digits=digits) == 0) &&
+        (round(y_proj_sum[2]; digits=digits) == 0)
+        y_θ = π
+    else
+        y_θ = atan(y_proj_sum[2], y_proj_sum[1])
+    end
+
+    return SVector(
+        project_back_from_unit_circle(x_θ, args.l),
+        project_back_from_unit_circle(y_θ, args.l),
+    )
+end
\ No newline at end of file