"""
Finite difference calculation & machine learning of electric potential & electric field from charge
"""

using LinearAlgebra
using Plots
using Random
using Flux
Random.seed!(1)
using Main.EquivariantOperators # end users should omit Main.

# make grid
dims = 2
dx = 0.1
cell = dx * Matrix(I, dims, dims)
rmax = 1.0
grid = Grid(cell, rmax)

# make operators
rmin = 1e-9
rmax = sqrt(3)
ϕ = Op(r -> 1 / (4π * r), rmin, rmax, cell)
E = Op(r -> 1 / (4π * r^2), rmin, rmax, cell; l = 1)
▽ = Op(:▽, cell)

# put dipole charges
ρf = zeros(size(grid)..., 1)
put!(ρf, grid, [0.5 0.0; -0.5 0.0]', [1.0 -1]'')

# calculate fields
Ef = E(ρf)
ϕf = ϕ(ρf)

# test
rvec = [0, 0]
@show get(ϕf, grid, rvec), [0.0]
@show get(Ef, grid, rvec), get(-▽(ϕ(ρf)), grid, rvec), [-2 / (4π * 0.5^2), 0]

p = []
push!(p, heatmap(ρf[:, :, 1]',title = "dipole charge"), )
push!(p, heatmap(ϕf[:, :, 1]', title = "dipole potential"),)
plot(p..., layout = length(p))
savefig("d1.svg");

# @unpack x, y=grid
x = grid.coords[:, :, 1]
y = grid.coords[:, :, 2]
s = 1e-2
u = s * Ef[:, :, 1]
v = s * Ef[:, :, 2]
x, y, u, v = vec.([x, y, u, v])
quiver(x, y, quiver = (u, v), c = :blue, title = "dipole electric field")
savefig("d2.svg");

##
# make neural operators
ϕ_ = Op(Radfunc(), rmin, rmax, cell)
E_ = Op(Radfunc(), rmin, rmax, cell; l = 1)

ps = Flux.params(ϕ_, E_)
function loss()
    remake!(E_)
    remake!(ϕ_)
    global E_f = E_(ρf)
    global ϕ_f = ϕ_(ρf)
    @show l = (nae(E_f, Ef) + nae(ϕ_f, ϕf)) / 2
    l
end

data = [()]
loss()
opt = ADAM(0.1)
Flux.@epochs 50 Flux.train!(loss, ps, data, opt)

## plot
p = []
push!(p, heatmap(ϕ_.kernel[:, :, 1], title = "learned potential kernel"))
r = 0:0.01:1
push!(
    p,
    plot(r, ϕ_.radfunc.(r), title = "learned potential kernel radial function"),
)
plot(p..., layout = length(p))
savefig("ml1.svg");

x = E_.grid.coords[:, :, 1]
y = E_.grid.coords[:, :, 2]
s = 1e-2
u = s * E_.kernel[:, :, 1]
v = s * E_.kernel[:, :, 2]
x, y, u, v = vec.([x, y, u, v])
quiver(x, y, quiver = (u, v), c = :blue, title = "learned E field kernel ")
savefig("ml2.svg");


struct SimpleStiffSystem
    λ::Float64
    de::Function
    function SimpleStiffSystem(de::Function)
        return new(1.0, de)
    end
end

function stiff_de!(xx::Vector{T}, x::Vector{T}, s::SimpleStiffSystem, t::Float64=0.0) where {T}
    xx .= -s.λ * x
    return nothing
end

my_stiff_struct = SimpleStiffSystem(stiff_de!)
x0 = zeros(Float64, 4) .+ 1.0
n_rule_max = 3

@testset "basic_test" begin
    for NC = 1:10  # check different chunk sizes
        rr = makeRadauIntegrator(my_stiff_struct, x0, 1.0e-16, n_rule_max, NC)
        for k_rule = 1:n_rule_max  # chick different rules
            rr.rule.s = 3
            t_final = 0.2
            update_h!(rr, t_final)
            h, x_final = solveRadau(rr, x0)
            if k_rule != 1
                x_ana = exp(-t_final)
                @test x_ana ≈ x_final[1]
            end
        end
    end
end


# implementation of the self-consistent-field (SCF) method

```
Form the density matrix R from the occupied columns of U. C contains (as columns) the MO
coefficients and the R-matrix is formed from the first nocc columns.

C - The eigenvectors are stored as columns of length m
R - The charge and bond order matrix generated is stored in R by columns (m x m)
m - The dimension of the matrices (basis set size)
nocc - The number of occupied orbitals (half of the number of eletrons in the closed-shell case)

```
function scfr(C, R, m::Int64, nocc::Int64)  # C and R : Matrix
  for i in 1:m
    for j in 1:i
      sum = 0
      for k in 1:nocc
        kk = m*(k-1)
        ik = kk + i
        jk = kk + j
        sum = sum + C[ik]*C[jk]
      end
      ij = m*(j - 1) + i
      ji = m*(i - 1) + j
      R[ij] = sum;
      R[ji] = sum;
    end
  end
end

export scfr


import LinearAlgebra as LA
import SimpleCollisions as SC
import StaticArrays as SA
import Test

Test.@testset "SimpleCollisions.jl" begin
    T = Float32
    VecType = SC.Vector2D{T}

    origin = zero(VecType)
    d = convert(T, 0.01)

    std_dir = SC.Vector2D(one(T), zero(T))
    i_cap = SC.Vector2D(one(T), zero(T))
    j_cap = SC.Vector2D(zero(T), one(T))

    theta = convert(T, π / 6)
    rotated_dir = SC.Vector2D(cos(theta), sin(theta))
    theta_45 = convert(T, π / 4)
    unit_45 = (i_cap + j_cap) / convert(T, sqrt(2))

    point = SC.StandardPoint{T}()

    half_length_l1 = one(T)
    l1 = SC.StandardLine(half_length_l1)
    p1_l1 = SC.get_tail(l1)
    p2_l1 = SC.get_head(l1)

    half_length_l2 = convert(T, 2)
    l2 = SC.StandardLine(half_length_l2)
    p1_l2 = SC.get_tail(l2)
    p2_l2 = SC.get_head(l2)

    r_c1 = one(T)
    c1 = SC.StandardCircle(r_c1)

    r_c2 = convert(T, 2)
    c2 = SC.StandardCircle(r_c2)

    half_width_r1 = one(T)
    half_height_r1 = convert(T, 0.5)
    r1 = SC.StandardRect(half_width_r1, half_height_r1)
    top_right_r1 = SC.get_top_right(r1)
    theta_r1 = atan(half_height_r1, half_width_r1)

    half_width_r2 = convert(T, 2)
    half_height_r2 = one(T)
    r2 = SC.StandardRect(half_width_r2, half_height_r2)
    top_right_r2 = SC.get_top_right(r2)
    theta_r2 = atan(half_height_r2, half_width_r2)

    Test.@testset "Area" begin
        Test.@testset "StandardRect" begin
            Test.@test SC.get_area(r2) == convert(T, 8)
        end

        Test.@testset "StandardCircle" begin
            Test.@test SC.get_area(c1) ≈ convert(T, π)
        end
    end

    Test.@testset "Collision detection" begin
        Test.@testset "StandardLine vs. StandardLine" begin
            Test.@testset "no dir" begin
                Test.@test SC.is_colliding(l1, l2, origin) == true
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 + d) * -i_cap) == false
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 - d) * -i_cap) == true
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 - d) * i_cap) == true
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 + d) * i_cap) == false
                Test.@test SC.is_colliding(l1, l2, d * -j_cap) == false
                Test.@test SC.is_colliding(l1, l2, d * j_cap) == false
            end

            Test.@testset "std_dir" begin
                Test.@test SC.is_colliding(l1, l2, origin, std_dir) == true
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(l1, l2, d * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(l1, l2, d * j_cap, std_dir) == false
            end

            Test.@testset "rotated_dir" begin
                Test.@test SC.is_colliding(l1, l2, origin, rotated_dir) == true
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 + d) * -i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 - d) * -i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + d) * -i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 - d) * -i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 - d) * i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + d) * i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 - d) * i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(l1, l2, (half_length_l1 + half_length_l2 + d) * i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(l2, l1, (half_length_l1 * sin(theta) + d) * -j_cap, rotated_dir) == false
                Test.@test SC.is_colliding(l2, l1, (half_length_l1 * sin(theta) - d) * -j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(l2, l1, d * -j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(l2, l1, d * j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(l2, l1, (half_length_l1 * sin(theta) - d) * j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(l2, l1, (half_length_l1 * sin(theta) + d) * j_cap, rotated_dir) == false
            end
        end

        Test.@testset "StandardCircle vs. StandardPoint" begin
            Test.@testset "std_dir" begin
                Test.@test SC.is_colliding(c1, point, origin, std_dir) == true
                Test.@test SC.is_colliding(c1, point, (r_c1 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, point, (r_c1 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, point, (r_c1 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, point, (r_c1 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, point, (r_c1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, point, (r_c1 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, point, (r_c1 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, point, (r_c1 + d) * j_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, point, (r_c1 + d) * unit_45, std_dir) == false
                Test.@test SC.is_colliding(c1, point, (r_c1 - d) * unit_45, std_dir) == true
            end

            Test.@testset "reverse check with std_dir" begin
                Test.@test SC.is_colliding(point, c1, origin, std_dir) == true
                Test.@test SC.is_colliding(point, c1, (r_c1 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(point, c1, (r_c1 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(point, c1, (r_c1 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(point, c1, (r_c1 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(point, c1, (r_c1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(point, c1, (r_c1 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(point, c1, (r_c1 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(point, c1, (r_c1 + d) * j_cap, std_dir) == false
                Test.@test SC.is_colliding(point, c1, (r_c1 + d) * unit_45, std_dir) == false
                Test.@test SC.is_colliding(point, c1, (r_c1 - d) * unit_45, std_dir) == true
            end

            Test.@testset "no dir" begin
                Test.@test SC.is_colliding(c1, point, origin) == true
                Test.@test SC.is_colliding(c1, point, (r_c1 + d) * -i_cap) == false
                Test.@test SC.is_colliding(c1, point, (r_c1 - d) * -i_cap) == true
                Test.@test SC.is_colliding(c1, point, (r_c1 - d) * i_cap) == true
                Test.@test SC.is_colliding(c1, point, (r_c1 + d) * i_cap) == false
                Test.@test SC.is_colliding(c1, point, (r_c1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(c1, point, (r_c1 - d) * -j_cap) == true
                Test.@test SC.is_colliding(c1, point, (r_c1 - d) * j_cap) == true
                Test.@test SC.is_colliding(c1, point, (r_c1 + d) * j_cap) == false
                Test.@test SC.is_colliding(c1, point, (r_c1 + d) * unit_45) == false
                Test.@test SC.is_colliding(c1, point, (r_c1 - d) * unit_45) == true
            end

            Test.@testset "reverse check with no dir" begin
                Test.@test SC.is_colliding(point, c1, origin) == true
                Test.@test SC.is_colliding(point, c1, (r_c1 + d) * -i_cap) == false
                Test.@test SC.is_colliding(point, c1, (r_c1 - d) * -i_cap) == true
                Test.@test SC.is_colliding(point, c1, (r_c1 - d) * i_cap) == true
                Test.@test SC.is_colliding(point, c1, (r_c1 + d) * i_cap) == false
                Test.@test SC.is_colliding(point, c1, (r_c1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(point, c1, (r_c1 - d) * -j_cap) == true
                Test.@test SC.is_colliding(point, c1, (r_c1 - d) * j_cap) == true
                Test.@test SC.is_colliding(point, c1, (r_c1 + d) * j_cap) == false
                Test.@test SC.is_colliding(point, c1, (r_c1 + d) * unit_45) == false
                Test.@test SC.is_colliding(point, c1, (r_c1 - d) * unit_45) == true
            end
        end

        Test.@testset "StandardCircle vs. StandardLine" begin
            Test.@testset "std_dir" begin
                Test.@test SC.is_colliding(l1, c2, origin, std_dir) == true
                Test.@test SC.is_colliding(l2, c1, origin, std_dir) == true
                Test.@test SC.is_colliding(l2, c1, (half_length_l2 + r_c1 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(l2, c1, (half_length_l2 + r_c1 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(l2, c1, (half_length_l2 + r_c1 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(l2, c1, (half_length_l2 + r_c1 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(l2, c1, (r_c1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(l2, c1, (r_c1 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(l2, c1, (r_c1 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(l2, c1, (r_c1 + d) * j_cap, std_dir) == false
            end

            Test.@testset "reverse check with std_dir" begin
                Test.@test SC.is_colliding(c2, l1, origin, std_dir) == true
                Test.@test SC.is_colliding(c1, l2, origin, std_dir) == true
                Test.@test SC.is_colliding(c1, l2, (half_length_l2 + r_c1 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, l2, (half_length_l2 + r_c1 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, l2, (half_length_l2 + r_c1 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, l2, (half_length_l2 + r_c1 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, l2, (r_c1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, l2, (r_c1 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, l2, (r_c1 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, l2, (r_c1 + d) * j_cap, std_dir) == false
            end

            Test.@testset "reverse check with rotated_dir" begin
                Test.@test SC.is_colliding(c2, l1, origin, rotated_dir) == true
                Test.@test SC.is_colliding(c1, l2, origin, rotated_dir) == true
                Test.@test SC.is_colliding(c1, l1, (sqrt(r_c1 ^ 2 - (half_length_l1 * sin(theta)) ^ 2) + half_length_l1 * cos(theta) + d) * -i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(c1, l1, (sqrt(r_c1 ^ 2 - (half_length_l1 * sin(theta)) ^ 2) + half_length_l1 * cos(theta) - d) * -i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(c1, l1, (sqrt(r_c1 ^ 2 - (half_length_l1 * sin(theta)) ^ 2) + half_length_l1 * cos(theta) - d) * i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(c1, l1, (sqrt(r_c1 ^ 2 - (half_length_l1 * sin(theta)) ^ 2) + half_length_l1 * cos(theta) + d) * i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(c1, l2, (r_c1 / cos(theta) + d) * -j_cap, rotated_dir) == false
                Test.@test SC.is_colliding(c1, l2, (r_c1 / cos(theta) - d) * -j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(c1, l2, (r_c1 / cos(theta) - d) * j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(c1, l2, (r_c1 / cos(theta) + d) * j_cap, rotated_dir) == false
            end

            Test.@testset "no dir" begin
                Test.@test SC.is_colliding(l1, c2, origin) == true
                Test.@test SC.is_colliding(l2, c1, origin) == true
                Test.@test SC.is_colliding(l2, c1, (half_length_l2 + r_c1 + d) * -i_cap) == false
                Test.@test SC.is_colliding(l2, c1, (half_length_l2 + r_c1 - d) * -i_cap) == true
                Test.@test SC.is_colliding(l2, c1, (half_length_l2 + r_c1 - d) * i_cap) == true
                Test.@test SC.is_colliding(l2, c1, (half_length_l2 + r_c1 + d) * i_cap) == false
                Test.@test SC.is_colliding(l2, c1, (r_c1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(l2, c1, (r_c1 - d) * -j_cap) == true
                Test.@test SC.is_colliding(l2, c1, (r_c1 - d) * j_cap) == true
                Test.@test SC.is_colliding(l2, c1, (r_c1 + d) * j_cap) == false
            end

            Test.@testset "reverse check with no dir" begin
                Test.@test SC.is_colliding(c2, l1, origin) == true
                Test.@test SC.is_colliding(c1, l2, origin) == true
                Test.@test SC.is_colliding(c1, l2, (half_length_l2 + r_c1 + d) * -i_cap) == false
                Test.@test SC.is_colliding(c1, l2, (half_length_l2 + r_c1 - d) * -i_cap) == true
                Test.@test SC.is_colliding(c1, l2, (half_length_l2 + r_c1 - d) * i_cap) == true
                Test.@test SC.is_colliding(c1, l2, (half_length_l2 + r_c1 + d) * i_cap) == false
                Test.@test SC.is_colliding(c1, l2, (r_c1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(c1, l2, (r_c1 - d) * -j_cap) == true
                Test.@test SC.is_colliding(c1, l2, (r_c1 - d) * j_cap) == true
                Test.@test SC.is_colliding(c1, l2, (r_c1 + d) * j_cap) == false
            end
        end

        Test.@testset "StandardCircle vs. StandardCircle" begin
            Test.@testset "std_dir" begin
                Test.@test SC.is_colliding(c1, c2, origin, std_dir) == true
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 + d) * j_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 + d) * unit_45, std_dir) == false
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 - d) * unit_45, std_dir) == true
            end

            Test.@testset "no dir" begin
                Test.@test SC.is_colliding(c1, c2, origin) == true
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 + d) * -i_cap) == false
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 - d) * -i_cap) == true
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 - d) * i_cap) == true
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 + d) * i_cap) == false
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 + d) * -j_cap) == false
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 - d) * -j_cap) == true
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 - d) * j_cap) == true
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 + d) * j_cap) == false
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 + d) * unit_45) == false
                Test.@test SC.is_colliding(c1, c2, (r_c1 + r_c2 - d) * unit_45) == true
            end
        end

        Test.@testset "StandardRect vs. StandardPoint" begin
            Test.@testset "std_dir" begin
                Test.@test SC.is_colliding(r1, point, origin, std_dir) == true
                Test.@test SC.is_colliding(r1, point, (half_width_r1 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, point, (half_width_r1 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, point, (half_width_r1 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, point, (half_width_r1 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, point, (half_height_r1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, point, (half_height_r1 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, point, (half_height_r1 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, point, (half_height_r1 + d) * j_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, point, top_right_r1 .+ d, std_dir) == false
                Test.@test SC.is_colliding(r1, point, top_right_r1 .- d, std_dir) == true
            end

            Test.@testset "reverse check with std_dir" begin
                Test.@test SC.is_colliding(point, r1, origin, std_dir) == true
                Test.@test SC.is_colliding(point, r1, (half_width_r1 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(point, r1, (half_width_r1 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(point, r1, (half_width_r1 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(point, r1, (half_width_r1 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(point, r1, (half_height_r1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(point, r1, (half_height_r1 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(point, r1, (half_height_r1 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(point, r1, (half_height_r1 + d) * j_cap, std_dir) == false
                Test.@test SC.is_colliding(point, r1, top_right_r1 .+ d, std_dir) == false
                Test.@test SC.is_colliding(point, r1, top_right_r1 .- d, std_dir) == true
            end

            Test.@testset "reverse check with rotated_dir" begin
                Test.@test SC.is_colliding(point, r1, origin, rotated_dir) == true
                Test.@test SC.is_colliding(point, r1, (half_height_r1 / sin(theta) + d) * -i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(point, r1, (half_height_r1 / sin(theta) - d) * -i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(point, r1, (half_height_r1 / sin(theta) - d) * i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(point, r1, (half_height_r1 / sin(theta) + d) * i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(point, r1, (half_height_r1 / cos(theta) + d) * -j_cap, rotated_dir) == false
                Test.@test SC.is_colliding(point, r1, (half_height_r1 / cos(theta) - d) * -j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(point, r1, (half_height_r1 / cos(theta) - d) * j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(point, r1, (half_height_r1 / cos(theta) + d) * j_cap, rotated_dir) == false
            end

            Test.@testset "no dir" begin
                Test.@test SC.is_colliding(r1, point, origin) == true
                Test.@test SC.is_colliding(r1, point, (half_width_r1 + d) * -i_cap) == false
                Test.@test SC.is_colliding(r1, point, (half_width_r1 - d) * -i_cap) == true
                Test.@test SC.is_colliding(r1, point, (half_width_r1 - d) * i_cap) == true
                Test.@test SC.is_colliding(r1, point, (half_width_r1 + d) * i_cap) == false
                Test.@test SC.is_colliding(r1, point, (half_height_r1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(r1, point, (half_height_r1 - d) * -j_cap) == true
                Test.@test SC.is_colliding(r1, point, (half_height_r1 - d) * j_cap) == true
                Test.@test SC.is_colliding(r1, point, (half_height_r1 + d) * j_cap) == false
                Test.@test SC.is_colliding(r1, point, top_right_r1 .+ d) == false
                Test.@test SC.is_colliding(r1, point, top_right_r1 .- d) == true
            end

            Test.@testset "reverse check with no dir" begin
                Test.@test SC.is_colliding(point, r1, origin) == true
                Test.@test SC.is_colliding(point, r1, (half_width_r1 + d) * -i_cap) == false
                Test.@test SC.is_colliding(point, r1, (half_width_r1 - d) * -i_cap) == true
                Test.@test SC.is_colliding(point, r1, (half_width_r1 - d) * i_cap) == true
                Test.@test SC.is_colliding(point, r1, (half_width_r1 + d) * i_cap) == false
                Test.@test SC.is_colliding(point, r1, (half_height_r1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(point, r1, (half_height_r1 - d) * -j_cap) == true
                Test.@test SC.is_colliding(point, r1, (half_height_r1 - d) * j_cap) == true
                Test.@test SC.is_colliding(point, r1, (half_height_r1 + d) * j_cap) == false
                Test.@test SC.is_colliding(point, r1, top_right_r1 .+ d) == false
                Test.@test SC.is_colliding(point, r1, top_right_r1 .- d) == true
            end
        end

        Test.@testset "StandardRect vs. StandardLine" begin
            Test.@testset "std_dir" begin
                Test.@test SC.is_colliding(r1, l1, origin, std_dir) == true
                Test.@test SC.is_colliding(r1, l2, origin, std_dir) == true
                Test.@test SC.is_colliding(r1, l1, (half_width_r1 + half_length_l1 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, l1, (half_width_r1 + half_length_l1 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, l1, (half_width_r1 + half_length_l1 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, l1, (half_width_r1 + half_length_l1 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, l1, (half_height_r1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, l1, (half_height_r1 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, l1, (half_height_r1 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, l1, (half_height_r1 + d) * j_cap, std_dir) == false
            end

            Test.@testset "rotated_dir" begin
                Test.@test SC.is_colliding(r1, l1, origin, rotated_dir) == true
                Test.@test SC.is_colliding(r1, l2, origin, rotated_dir) == true
                Test.@test SC.is_colliding(r2, l1, (half_width_r2 + half_length_l1 * cos(theta) + d) * -i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(r2, l1, (half_width_r2 + half_length_l1 * cos(theta) - d) * -i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(r2, l1, (half_width_r2 + half_length_l1 * cos(theta) - d) * i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(r2, l1, (half_width_r2 + half_length_l1 * cos(theta) + d) * i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(r2, l1, (half_height_r2 + half_length_l1 * sin(theta) + d) * -j_cap, rotated_dir) == false
                Test.@test SC.is_colliding(r2, l1, (half_height_r2 + half_length_l1 * sin(theta) - d) * -j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(r2, l1, (half_height_r2 + half_length_l1 * sin(theta) - d) * j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(r2, l1, (half_height_r2 + half_length_l1 * sin(theta) + d) * j_cap, rotated_dir) == false
            end

            Test.@testset "reverse check with std_dir" begin
                Test.@test SC.is_colliding(l1, r1, origin, std_dir) == true
                Test.@test SC.is_colliding(l2, r1, origin, std_dir) == true
                Test.@test SC.is_colliding(l1, r1, (half_width_r1 + half_length_l1 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(l1, r1, (half_width_r1 + half_length_l1 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(l1, r1, (half_width_r1 + half_length_l1 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(l1, r1, (half_width_r1 + half_length_l1 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(l1, r1, (half_height_r1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(l1, r1, (half_height_r1 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(l1, r1, (half_height_r1 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(l1, r1, (half_height_r1 + d) * j_cap, std_dir) == false
            end

            Test.@testset "reverse check with rotated_dir" begin
                Test.@test SC.is_colliding(l1, r1, origin, rotated_dir) == true
                Test.@test SC.is_colliding(l2, r1, origin, rotated_dir) == true
                Test.@test SC.is_colliding(l2, r2, (half_height_r2 / sin(theta) + half_length_l2 + d) * -i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(l2, r2, (half_height_r2 / sin(theta) + half_length_l2 - d) * -i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(l2, r2, (half_height_r2 / sin(theta) + half_length_l2 - d) * i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(l2, r2, (half_height_r2 / sin(theta) + half_length_l2 + d) * i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(l2, r2, (half_width_r2 * sin(theta) + half_height_r2 * cos(theta) + d) * -j_cap, rotated_dir) == false
                Test.@test SC.is_colliding(l2, r2, (half_width_r2 * sin(theta) + half_height_r2 * cos(theta) - d) * -j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(l2, r2, (half_width_r2 * sin(theta) + half_height_r2 * cos(theta) - d) * j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(l2, r2, (half_width_r2 * sin(theta) + half_height_r2 * cos(theta) + d) * j_cap, rotated_dir) == false
            end

            Test.@testset "no dir" begin
                Test.@test SC.is_colliding(r1, l1, origin) == true
                Test.@test SC.is_colliding(r1, l2, origin) == true
                Test.@test SC.is_colliding(r1, l1, (half_width_r1 + half_length_l1 + d) * -i_cap) == false
                Test.@test SC.is_colliding(r1, l1, (half_width_r1 + half_length_l1 - d) * -i_cap) == true
                Test.@test SC.is_colliding(r1, l1, (half_width_r1 + half_length_l1 - d) * i_cap) == true
                Test.@test SC.is_colliding(r1, l1, (half_width_r1 + half_length_l1 + d) * i_cap) == false
                Test.@test SC.is_colliding(r1, l1, (half_height_r1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(r1, l1, (half_height_r1 - d) * -j_cap) == true
                Test.@test SC.is_colliding(r1, l1, (half_height_r1 - d) * j_cap) == true
                Test.@test SC.is_colliding(r1, l1, (half_height_r1 + d) * j_cap) == false
            end

            Test.@testset "reverse check with no dir" begin
                Test.@test SC.is_colliding(l1, r1, origin) == true
                Test.@test SC.is_colliding(l2, r1, origin) == true
                Test.@test SC.is_colliding(l1, r1, (half_width_r1 + half_length_l1 + d) * -i_cap) == false
                Test.@test SC.is_colliding(l1, r1, (half_width_r1 + half_length_l1 - d) * -i_cap) == true
                Test.@test SC.is_colliding(l1, r1, (half_width_r1 + half_length_l1 - d) * i_cap) == true
                Test.@test SC.is_colliding(l1, r1, (half_width_r1 + half_length_l1 + d) * i_cap) == false
                Test.@test SC.is_colliding(l1, r1, (half_height_r1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(l1, r1, (half_height_r1 - d) * -j_cap) == true
                Test.@test SC.is_colliding(l1, r1, (half_height_r1 - d) * j_cap) == true
                Test.@test SC.is_colliding(l1, r1, (half_height_r1 + d) * j_cap) == false
            end
        end

        Test.@testset "StandardRect vs. StandardCircle" begin
            Test.@testset "std_dir" begin
                Test.@test SC.is_colliding(r1, c1, origin, std_dir) == true
                Test.@test SC.is_colliding(r1, c1, (half_width_r1 + r_c1 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, c1, (half_width_r1 + r_c1 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, c1, (half_width_r1 + r_c1 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, c1, (half_width_r1 + r_c1 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, c1, (half_height_r1 + r_c1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, c1, (half_height_r1 + r_c1 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, c1, (half_height_r1 + r_c1 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(r1, c1, (half_height_r1 + r_c1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(r1, c1, top_right_r1 + (r_c1 + d) * unit_45, std_dir) == false
                Test.@test SC.is_colliding(r1, c1, top_right_r1 + (r_c1 - d) * unit_45, std_dir) == true
            end

            Test.@testset "reverse check with std_dir" begin
                Test.@test SC.is_colliding(c1, r1, origin, std_dir) == true
                Test.@test SC.is_colliding(c1, r1, (half_width_r1 + r_c1 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, r1, (half_width_r1 + r_c1 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, r1, (half_width_r1 + r_c1 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, r1, (half_width_r1 + r_c1 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, r1, (half_height_r1 + r_c1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, r1, (half_height_r1 + r_c1 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, r1, (half_height_r1 + r_c1 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(c1, r1, (half_height_r1 + r_c1 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(c1, r1, top_right_r1 + (r_c1 + d) * unit_45, std_dir) == false
                Test.@test SC.is_colliding(c1, r1, top_right_r1 + (r_c1 - d) * unit_45, std_dir) == true
            end

            Test.@testset "reverse check with rotated_dir" begin
                Test.@test SC.is_colliding(c2, r2, origin, rotated_dir) == true
                Test.@test SC.is_colliding(c2, r2, (sqrt(r_c2 ^ 2 - (half_width_r2 * sin(theta) - half_height_r2 * cos(theta)) ^ 2) + half_width_r2 * cos(theta) + half_height_r2 * sin(theta) + d) * -i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(c2, r2, (sqrt(r_c2 ^ 2 - (half_width_r2 * sin(theta) - half_height_r2 * cos(theta)) ^ 2) + half_width_r2 * cos(theta) + half_height_r2 * sin(theta) - d) * -i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(c2, r2, (sqrt(r_c2 ^ 2 - (half_width_r2 * sin(theta) - half_height_r2 * cos(theta)) ^ 2) + half_width_r2 * cos(theta) + half_height_r2 * sin(theta) - d) * i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(c2, r2, (sqrt(r_c2 ^ 2 - (half_width_r2 * sin(theta) - half_height_r2 * cos(theta)) ^ 2) + half_width_r2 * cos(theta) + half_height_r2 * sin(theta) + d) * i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(c2, r2, ((r_c2 + half_height_r2) / cos(theta) + d) * -j_cap, rotated_dir) == false
                Test.@test SC.is_colliding(c2, r2, ((r_c2 + half_height_r2) / cos(theta) - d) * -j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(c2, r2, ((r_c2 + half_height_r2) / cos(theta) - d) * j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(c2, r2, ((r_c2 + half_height_r2) / cos(theta) + d) * j_cap, rotated_dir) == false
            end

            Test.@testset "no dir" begin
                Test.@test SC.is_colliding(r1, c1, origin) == true
                Test.@test SC.is_colliding(r1, c1, (half_width_r1 + r_c1 + d) * -i_cap) == false
                Test.@test SC.is_colliding(r1, c1, (half_width_r1 + r_c1 - d) * -i_cap) == true
                Test.@test SC.is_colliding(r1, c1, (half_width_r1 + r_c1 - d) * i_cap) == true
                Test.@test SC.is_colliding(r1, c1, (half_width_r1 + r_c1 + d) * i_cap) == false
                Test.@test SC.is_colliding(r1, c1, (half_height_r1 + r_c1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(r1, c1, (half_height_r1 + r_c1 - d) * -j_cap) == true
                Test.@test SC.is_colliding(r1, c1, (half_height_r1 + r_c1 - d) * j_cap) == true
                Test.@test SC.is_colliding(r1, c1, (half_height_r1 + r_c1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(r1, c1, top_right_r1 + (r_c1 + d) * unit_45) == false
                Test.@test SC.is_colliding(r1, c1, top_right_r1 + (r_c1 - d) * unit_45) == true
            end

            Test.@testset "reverse check with no dir" begin
                Test.@test SC.is_colliding(c1, r1, origin) == true
                Test.@test SC.is_colliding(c1, r1, (half_width_r1 + r_c1 + d) * -i_cap) == false
                Test.@test SC.is_colliding(c1, r1, (half_width_r1 + r_c1 - d) * -i_cap) == true
                Test.@test SC.is_colliding(c1, r1, (half_width_r1 + r_c1 - d) * i_cap) == true
                Test.@test SC.is_colliding(c1, r1, (half_width_r1 + r_c1 + d) * i_cap) == false
                Test.@test SC.is_colliding(c1, r1, (half_height_r1 + r_c1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(c1, r1, (half_height_r1 + r_c1 - d) * -j_cap) == true
                Test.@test SC.is_colliding(c1, r1, (half_height_r1 + r_c1 - d) * j_cap) == true
                Test.@test SC.is_colliding(c1, r1, (half_height_r1 + r_c1 + d) * -j_cap) == false
                Test.@test SC.is_colliding(c1, r1, top_right_r1 + (r_c1 + d) * unit_45) == false
                Test.@test SC.is_colliding(c1, r1, top_right_r1 + (r_c1 - d) * unit_45) == true
            end
        end

        Test.@testset "Rect2D vs. Rect2D" begin
            Test.@testset "std_dir" begin
                Test.@test SC.is_colliding(r2, r1, origin, std_dir) == true
                Test.@test SC.is_colliding(r2, r1, (half_width_r1 + half_width_r2 + d) * -i_cap, std_dir) == false
                Test.@test SC.is_colliding(r2, r1, (half_width_r1 + half_width_r2 - d) * -i_cap, std_dir) == true
                Test.@test SC.is_colliding(r2, r1, (half_width_r1 + half_width_r2 - d) * i_cap, std_dir) == true
                Test.@test SC.is_colliding(r2, r1, (half_width_r1 + half_width_r2 + d) * i_cap, std_dir) == false
                Test.@test SC.is_colliding(r2, r1, (half_height_r1 + half_height_r2 + d) * -j_cap, std_dir) == false
                Test.@test SC.is_colliding(r2, r1, (half_height_r1 + half_height_r2 - d) * -j_cap, std_dir) == true
                Test.@test SC.is_colliding(r2, r1, (half_height_r1 + half_height_r2 - d) * j_cap, std_dir) == true
                Test.@test SC.is_colliding(r2, r1, (half_height_r1 + half_height_r2 + d) * j_cap, std_dir) == false
                Test.@test SC.is_colliding(r2, r1, top_right_r1 + top_right_r2 .+ d, std_dir) == false
                Test.@test SC.is_colliding(r2, r1, top_right_r1 + top_right_r2 .- d, std_dir) == true
            end

            Test.@testset "rotated_dir" begin
                Test.@test SC.is_colliding(r2, r1, origin, rotated_dir) == true
                Test.@test SC.is_colliding(r2, r1, (half_width_r2 + half_width_r1 * cos(theta) + half_height_r1 * sin(theta) + d) * -i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(r2, r1, (half_width_r2 + half_width_r1 * cos(theta) + half_height_r1 * sin(theta) - d) * -i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(r2, r1, (half_width_r2 + half_width_r1 * cos(theta) + half_height_r1 * sin(theta) - d) * i_cap, rotated_dir) == true
                Test.@test SC.is_colliding(r2, r1, (half_width_r2 + half_width_r1 * cos(theta) + half_height_r1 * sin(theta) + d) * i_cap, rotated_dir) == false
                Test.@test SC.is_colliding(r2, r1, (half_height_r2 + half_width_r1 * sin(theta) + half_height_r1 * cos(theta) + d) * -j_cap, rotated_dir) == false
                Test.@test SC.is_colliding(r2, r1, (half_height_r2 + half_width_r1 * sin(theta) + half_height_r1 * cos(theta) - d) * -j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(r2, r1, (half_height_r2 + half_width_r1 * sin(theta) + half_height_r1 * cos(theta) - d) * j_cap, rotated_dir) == true
                Test.@test SC.is_colliding(r2, r1, (half_height_r2 + half_width_r1 * sin(theta) + half_height_r1 * cos(theta) + d) * j_cap, rotated_dir) == false
            end

            Test.@testset "no dir" begin
                Test.@test SC.is_colliding(r2, r1, origin) == true
                Test.@test SC.is_colliding(r2, r1, (half_width_r1 + half_width_r2 + d) * -i_cap) == false
                Test.@test SC.is_colliding(r2, r1, (half_width_r1 + half_width_r2 - d) * -i_cap) == true
                Test.@test SC.is_colliding(r2, r1, (half_width_r1 + half_width_r2 - d) * i_cap) == true
                Test.@test SC.is_colliding(r2, r1, (half_width_r1 + half_width_r2 + d) * i_cap) == false
                Test.@test SC.is_colliding(r2, r1, (half_height_r1 + half_height_r2 + d) * -j_cap) == false
                Test.@test SC.is_colliding(r2, r1, (half_height_r1 + half_height_r2 - d) * -j_cap) == true
                Test.@test SC.is_colliding(r2, r1, (half_height_r1 + half_height_r2 - d) * j_cap) == true
                Test.@test SC.is_colliding(r2, r1, (half_height_r1 + half_height_r2 + d) * j_cap) == false
                Test.@test SC.is_colliding(r2, r1, top_right_r1 + top_right_r2 .+ d) == false
                Test.@test SC.is_colliding(r2, r1, top_right_r1 + top_right_r2 .- d) == true
            end
        end
    end

    Test.@testset "Manifold generation" begin
        Test.@testset "StandardCircle vs. StandardCircle" begin
            Test.@testset "std_dir" begin
                manifold_calculated = SC.Manifold(c1, c2, (r_c1 + r_c2 - d) * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, -i_cap, (r_c1 - d / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, c2, r_c2 * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1, -i_cap, (r_c1 / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, c2, r_c2 * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1, i_cap, (r_c1 / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, c2, (r_c1 + r_c2 - d) * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, i_cap, (r_c1 - d / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, c2, (r_c1 + r_c2 - d) * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, -j_cap, (r_c1 - d / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, c2, r_c2 * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1, -j_cap, (r_c1 / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, c2, r_c2 * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1, j_cap, (r_c1 / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, c2, (r_c1 + r_c2 - d) * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, j_cap, (r_c1 - d / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, c2, (r_c1 + r_c2 - d) * unit_45, std_dir)
                manifold_ground_truth = SC.Manifold(d, unit_45, (r_c1 - d / 2) * unit_45)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, c2, r_c2 * unit_45, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1, unit_45, (r_c1 / 2) * unit_45)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))
            end
        end

        Test.@testset "StandardRect vs. StandardCircle" begin
            Test.@testset "no dir" begin
                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 + r_c1 - d) * -i_cap)
                manifold_ground_truth = SC.Manifold(d, -i_cap, (half_width_r1 - d / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 + d) * -i_cap)
                manifold_ground_truth = SC.Manifold(r_c1 - d, -i_cap, (half_width_r1 - (r_c1 - d) / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 - d) * -i_cap)
                manifold_ground_truth = SC.Manifold(r_c1 + d, -i_cap, (half_width_r1 - (r_c1 + d) / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 - d) * i_cap)
                manifold_ground_truth = SC.Manifold(r_c1 + d, i_cap, (half_width_r1 - (r_c1 + d) / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 + d) * i_cap)
                manifold_ground_truth = SC.Manifold(r_c1 - d, i_cap, (half_width_r1 - (r_c1 - d) / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 + r_c1 - d) * i_cap)
                manifold_ground_truth = SC.Manifold(d, i_cap, (half_width_r1 - d / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 + r_c1 - d) * -j_cap)
                manifold_ground_truth = SC.Manifold(d, -j_cap, (half_height_r1 - d / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 + d) * -j_cap)
                manifold_ground_truth = SC.Manifold(r_c1 - d, -j_cap, (half_height_r1 - (r_c1 - d) / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 - d) * -j_cap)
                manifold_ground_truth = SC.Manifold(r_c1 + d, -j_cap, (half_height_r1 - (r_c1 + d) / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 - d) * j_cap)
                manifold_ground_truth = SC.Manifold(r_c1 + d, j_cap, (half_height_r1 - (r_c1 + d) / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 + d) * j_cap)
                manifold_ground_truth = SC.Manifold(r_c1 - d, j_cap, (half_height_r1 - (r_c1 - d) / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 + r_c1 - d) * j_cap)
                manifold_ground_truth = SC.Manifold(d, j_cap, (half_height_r1 - d / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, top_right_r1 + (r_c1 - d) * unit_45)
                manifold_ground_truth = SC.Manifold(d, unit_45, top_right_r1 + (d / 2) * -unit_45)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))
            end

            Test.@testset "reverse check with no dir" begin
                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 + r_c1 - d) * -i_cap)
                manifold_ground_truth = SC.Manifold(d, -i_cap, (r_c1 - d / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 + d) * -i_cap)
                manifold_ground_truth = SC.Manifold(r_c1 - d, -i_cap, (r_c1 - (r_c1 - d) / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 - d) * -i_cap)
                manifold_ground_truth = SC.Manifold(r_c1 + d, -i_cap, (r_c1 - (r_c1 + d) / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 - d) * i_cap)
                manifold_ground_truth = SC.Manifold(r_c1 + d, i_cap, (r_c1 - (r_c1 + d) / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 + d) * i_cap)
                manifold_ground_truth = SC.Manifold(r_c1 - d, i_cap, (r_c1 - (r_c1 - d) / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 + r_c1 - d) * i_cap)
                manifold_ground_truth = SC.Manifold(d, i_cap, (r_c1 - d / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 + r_c1 - d) * -j_cap)
                manifold_ground_truth = SC.Manifold(d, -j_cap, (r_c1 - d / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 + d) * -j_cap)
                manifold_ground_truth = SC.Manifold(r_c1 - d, -j_cap, (r_c1 - (r_c1 - d) / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 - d) * -j_cap)
                manifold_ground_truth = SC.Manifold(r_c1 + d, -j_cap, (r_c1 - (r_c1 + d) / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 - d) * j_cap)
                manifold_ground_truth = SC.Manifold(r_c1 + d, j_cap, (r_c1 - (r_c1 + d) / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 + d) * j_cap)
                manifold_ground_truth = SC.Manifold(r_c1 - d, j_cap, (r_c1 - (r_c1 - d) / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 + r_c1 - d) * j_cap)
                manifold_ground_truth = SC.Manifold(d, j_cap, (r_c1 - d / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, top_right_r1 + (r_c1 - d) * unit_45)
                manifold_ground_truth = SC.Manifold(d, unit_45, (r_c1 - d / 2) * unit_45)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))
            end

            Test.@testset "std_dir" begin
                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 + r_c1 - d) * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, -i_cap, (half_width_r1 - d / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 + d) * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 - d, -i_cap, (half_width_r1 - (r_c1 - d) / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 - d) * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 + d, -i_cap, (half_width_r1 - (r_c1 + d) / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 - d) * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 + d, i_cap, (half_width_r1 - (r_c1 + d) / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 + d) * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 - d, i_cap, (half_width_r1 - (r_c1 - d) / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_width_r1 + r_c1 - d) * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, i_cap, (half_width_r1 - d / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 + r_c1 - d) * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, -j_cap, (half_height_r1 - d / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 + d) * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 - d, -j_cap, (half_height_r1 - (r_c1 - d) / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 - d) * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 + d, -j_cap, (half_height_r1 - (r_c1 + d) / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 - d) * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 + d, j_cap, (half_height_r1 - (r_c1 + d) / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 + d) * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 - d, j_cap, (half_height_r1 - (r_c1 - d) / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, (half_height_r1 + r_c1 - d) * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, j_cap, (half_height_r1 - d / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r1, c1, top_right_r1 + (r_c1 - d) * unit_45, std_dir)
                manifold_ground_truth = SC.Manifold(d, unit_45, top_right_r1 + (d / 2) * -unit_45)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))
            end

            Test.@testset "reverse check with std_dir" begin
                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 + r_c1 - d) * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, -i_cap, (r_c1 - d / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 + d) * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 - d, -i_cap, (r_c1 - (r_c1 - d) / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 - d) * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 + d, -i_cap, (r_c1 - (r_c1 + d) / 2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 - d) * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 + d, i_cap, (r_c1 - (r_c1 + d) / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 + d) * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 - d, i_cap, (r_c1 - (r_c1 - d) / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_width_r1 + r_c1 - d) * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, i_cap, (r_c1 - d / 2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 + r_c1 - d) * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, -j_cap, (r_c1 - d / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 + d) * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 - d, -j_cap, (r_c1 - (r_c1 - d) / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 - d) * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 + d, -j_cap, (r_c1 - (r_c1 + d) / 2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 - d) * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 + d, j_cap, (r_c1 - (r_c1 + d) / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 + d) * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(r_c1 - d, j_cap, (r_c1 - (r_c1 - d) / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, (half_height_r1 + r_c1 - d) * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, j_cap, (r_c1 - d / 2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(c1, r1, top_right_r1 + (r_c1 - d) * unit_45, std_dir)
                manifold_ground_truth = SC.Manifold(d, unit_45, (r_c1 - d / 2) * unit_45)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))
            end

            Test.@testset "reverse check with rotated_dir" begin
                manifold_calculated = SC.Manifold(c1, r1, (r_c1 - d) * unit_45 + SC.rotate(top_right_r1, rotated_dir), rotated_dir)
                manifold_ground_truth = SC.Manifold(d, unit_45, (r_c1 - d / 2) * unit_45)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))
            end
        end

        Test.@testset "Rect2D vs. Rect2D" begin
            Test.@testset "no dir" begin
                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 + half_height_r1 - d) * -j_cap)
                manifold_ground_truth = SC.Manifold(d, -j_cap, (half_height_r2 - d/2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 + d) * -j_cap)
                manifold_ground_truth = SC.Manifold(half_height_r1 - d, -j_cap, (half_height_r2 - (half_height_r1 - d)/2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 - d) * -j_cap)
                manifold_ground_truth = SC.Manifold(half_height_r1 + d, -j_cap, (half_height_r2 - (half_height_r1 + d)/2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, d * -j_cap)
                manifold_ground_truth = SC.Manifold(half_height_r2 + half_height_r1 - d, -j_cap, d * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, d * j_cap)
                manifold_ground_truth = SC.Manifold(half_height_r2 + half_height_r1 - d, j_cap, d * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 - d) * j_cap)
                manifold_ground_truth = SC.Manifold(half_height_r1 + d, j_cap, (half_height_r2 - (half_height_r1 + d)/2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 + d) * j_cap)
                manifold_ground_truth = SC.Manifold(half_height_r1 - d, j_cap, (half_height_r2 - (half_height_r1 - d)/2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 + half_height_r1 - d) * j_cap)
                manifold_ground_truth = SC.Manifold(d, j_cap, (half_height_r2 - d/2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 + half_width_r1 - d) * -i_cap)
                manifold_ground_truth = SC.Manifold(d, -i_cap, (half_width_r2 - d/2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 + d) * -i_cap)
                manifold_ground_truth = SC.Manifold(half_width_r1 - d, -i_cap, (half_width_r2 - (half_width_r1 - d)/2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 - d) * -i_cap)
                manifold_ground_truth = SC.Manifold(half_width_r1 + d, -i_cap, (half_width_r2 - (half_width_r1 + d)/2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 - d) * i_cap)
                manifold_ground_truth = SC.Manifold(half_width_r1 + d, i_cap, (half_width_r2 - (half_width_r1 + d)/2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 + d) * i_cap)
                manifold_ground_truth = SC.Manifold(half_width_r1 - d, i_cap, (half_width_r2 - (half_width_r1 - d)/2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 + half_width_r1 - d) * i_cap)
                manifold_ground_truth = SC.Manifold(d, i_cap, (half_width_r2 - d/2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, top_right_r2 .- d)
                manifold_ground_truth = SC.Manifold(half_height_r1 + d, j_cap, top_right_r2 + (half_width_r1 + d)/2 * -i_cap + (half_height_r1 + d)/2 * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, top_right_r2)
                manifold_ground_truth = SC.Manifold(half_height_r1, j_cap, top_right_r2 + (half_width_r1/2) * -i_cap + (half_height_r1/2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, top_right_r2 .+ d)
                manifold_ground_truth = SC.Manifold(half_height_r1 - d, j_cap, top_right_r2 + (half_width_r1 - d)/2 * -i_cap + (half_height_r1 - d)/2 * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))
            end

            Test.@testset "std_dir" begin
                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 + half_height_r1 - d) * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, -j_cap, (half_height_r2 - d/2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 + d) * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(half_height_r1 - d, -j_cap, (half_height_r2 - (half_height_r1 - d)/2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 - d) * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(half_height_r1 + d, -j_cap, (half_height_r2 - (half_height_r1 + d)/2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, d * -j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(half_height_r2 + half_height_r1 - d, -j_cap, d * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, d * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(half_height_r2 + half_height_r1 - d, j_cap, d * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 - d) * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(half_height_r1 + d, j_cap, (half_height_r2 - (half_height_r1 + d)/2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 + d) * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(half_height_r1 - d, j_cap, (half_height_r2 - (half_height_r1 - d)/2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 + half_height_r1 - d) * j_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, j_cap, (half_height_r2 - d/2) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 + half_width_r1 - d) * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, -i_cap, (half_width_r2 - d/2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 + d) * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(half_width_r1 - d, -i_cap, (half_width_r2 - (half_width_r1 - d)/2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 - d) * -i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(half_width_r1 + d, -i_cap, (half_width_r2 - (half_width_r1 + d)/2) * -i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 - d) * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(half_width_r1 + d, i_cap, (half_width_r2 - (half_width_r1 + d)/2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 + d) * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(half_width_r1 - d, i_cap, (half_width_r2 - (half_width_r1 - d)/2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 + half_width_r1 - d) * i_cap, std_dir)
                manifold_ground_truth = SC.Manifold(d, i_cap, (half_width_r2 - d/2) * i_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, top_right_r2 .- d, std_dir)
                manifold_ground_truth = SC.Manifold(half_height_r1 + d, j_cap, top_right_r2 + (half_width_r1 + d)/2 * -i_cap + (half_height_r1 + d)/2 * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, top_right_r2, std_dir)
                manifold_ground_truth = SC.Manifold(half_height_r1, j_cap, top_right_r2 + (half_width_r1/2) * -i_cap + (half_height_r1/2) * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, top_right_r2 .+ d, std_dir)
                manifold_ground_truth = SC.Manifold(half_height_r1 - d, j_cap, top_right_r2 + (half_width_r1 - d)/2 * -i_cap + (half_height_r1 - d)/2 * -j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))
            end

            Test.@testset "rotated_dir" begin
                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 - d) * -j_cap - SC.rotate(top_right_r1, rotated_dir), rotated_dir)
                manifold_ground_truth = SC.Manifold(d, -j_cap, ((zero(T) - d / tan(theta) + d * tan(theta)) / 3) * i_cap + ((-half_height_r2 + d - half_height_r2 - half_height_r2) / 3) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_height_r2 - d) * j_cap + SC.rotate(top_right_r1, rotated_dir), rotated_dir)
                manifold_ground_truth = SC.Manifold(d, j_cap, ((zero(T) + d / tan(theta) - d * tan(theta)) / 3) * i_cap + ((half_height_r2 - d + half_height_r2 + half_height_r2) / 3) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 + LA.norm(top_right_r1) * cos(-theta_r1 + theta) - d) * -i_cap, rotated_dir)
                manifold_ground_truth = SC.Manifold(d, -i_cap, ((-half_width_r2 + d - half_width_r2 - half_width_r2) / 3) * i_cap + (LA.norm(top_right_r1) * sin(-theta_r1 + theta) + (d / tan(theta) - d * tan(theta)) / 3) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))

                manifold_calculated = SC.Manifold(r2, r1, (half_width_r2 + LA.norm(top_right_r1) * cos(-theta_r1 + theta) - d) * i_cap, rotated_dir)
                manifold_ground_truth = SC.Manifold(d, i_cap, ((half_width_r2 - d + half_width_r2 + half_width_r2) / 3) * i_cap + (LA.norm(top_right_r1) * sin(convert(T, pi - theta_r1) + theta) + (d * tan(theta) - d / tan(theta)) / 3) * j_cap)
                Test.@test isapprox(SC.get_penetration(manifold_calculated), SC.get_penetration(manifold_ground_truth))
                Test.@test isapprox(SC.get_normal(manifold_calculated), SC.get_normal(manifold_ground_truth))
                Test.@test isapprox(SC.get_contact(manifold_calculated), SC.get_contact(manifold_ground_truth))
            end
        end
    end
end


function marAIC(m::LinearMixedModel)

    rho = length(MixedModels.θ(m))+length(fixef(m))+1
    llk = -objective(m)/2.0

    cr=[1.0, 0.0]

   val=-2.0*llk+2.0*(cr[1]*rho+cr[2])

    IC(llk, rho, "MarAIC", cr, val  )
end

function marBIC(m::LinearMixedModel)

    rho = length(MixedModels.θ(m))+length(fixef(m))+1
    llk = -objective(m)/2.0
    n,p,q,k = size(m)
    #(nn,pp,qq,tt)=size(m)

    cr=[1.0, 0.0]

    val=-2.0*llk+log(n)*(cr[1]*rho+cr[2])

    IC(llk, rho, "MarBIC", cr, val )
end