dynamicFriction.jl

# Amira Abdel-Rahman
# (c) Massachusetts Institute of Technology 2020


using LinearAlgebra
using Plots
import JSON
# using Quaternions
using StaticArrays
# using Distributed, Rotations
using StaticArrays, BenchmarkTools
using Base.Threads
using CUDAnative
using CuArrays,CUDAdrv 
using Test
import Base: +, * , -, ^

#####################################################
struct Vector3
    x::Float64
    y::Float64
    z::Float64
    function Vector3()
        x=0.0
        y=0.0
        z=0.0
        new(x,y,z)
    end
    function Vector3(x,y,z)
       new(x,y,z)
    end
end
struct Quaternion
    x::Float64
    y::Float64
    z::Float64
    w::Float64
    function Quaternion()
        x=0.0
        y=0.0
        z=0.0
        w=1.0
        new(x,y,z,w)
    end
    function Quaternion(x,y,z,w)
        new(x,y,z,w)
    end
end
struct RotationMatrix
    te1::Float64
    te2::Float64
    te3::Float64
    te4::Float64
    te5::Float64
    te6::Float64
    te7::Float64
    te8::Float64
    te9::Float64
    te10::Float64
    te11::Float64
    te12::Float64
    te13::Float64
    te14::Float64
    te15::Float64
    te16::Float64
    function RotationMatrix()
        te1 =0.0
        te2 =0.0
        te3 =0.0
        te4 =0.0
        te5 =0.0
        te6 =0.0
        te7 =0.0
        te8 =0.0
        te9 =0.0
        te10=0.0
        te11=0.0
        te12=0.0
        te13=0.0
        te14=0.0
        te15=0.0
        te16=0.0
        new(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
    end
    function RotationMatrix(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
        new(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)
    end
end

+(f::Vector3, g::Vector3)=Vector3(f.x+g.x , f.y+g.y,f.z+g.z )
-(f::Vector3, g::Vector3)=Vector3(f.x-g.x , f.y-g.y,f.z-g.z )
*(f::Vector3, g::Vector3)=Vector3(f.x*g.x , f.y*g.y,f.z*g.z )

+(f::Vector3, g::Number)=Vector3(f.x+g , f.y+g,f.z+g )
-(f::Vector3, g::Number)=Vector3(f.x-g , f.y-g,f.z-g )
*(f::Vector3, g::Number)=Vector3(f.x*g , f.y*g,f.z*g )

+(g::Vector3, f::Number)=Vector3(f.x+g , f.y+g,f.z+g )
-(g::Vector3, f::Number)=Vector3(g-f.x , g-f.y,g-f.z )
*(g::Vector3, f::Number)=Vector3(f.x*g , f.y*g,f.z*g )

addX(f::Vector3, g::Number)=Vector3(f.x+g , f.y,f.z)
addY(f::Vector3, g::Number)=Vector3(f.x , f.y+g,f.z )
addZ(f::Vector3, g::Number)=Vector3(f.x , f.y,f.z+g )

function normalizeVector3(f::Vector3)
    leng=sqrt((f.x * f.x) + (f.y * f.y) + (f.z * f.z))
    return Vector3(f.x/leng,f.y/leng,f.z/leng)
    
end
function normalizeQuaternion(f::Quaternion)
    l = sqrt((f.x * f.x) + (f.y * f.y) + (f.z * f.z)+ (f.w * f.w))
    if l === 0 
        qx = 0
        qy = 0
        qz = 0
        qw = 1
    else 
        l = 1 / l
        qx = f.x * l
        qy = f.y * l
        qz = f.z * l
        qw = f.w * l
    end
    return Quaternion(qx,qy,qz,qw)
end

function normalizeQuaternion1!(fx::Float64,fy::Float64,fz::Float64,fw::Float64)
    l = sqrt((fx * fx) + (fy * fy) + (fz * fz)+ (fw * fw))
    if l === 0 
        qx = 0.0
        qy = 0.0
        qz = 0.0
        qw = 1.0
    else 
        l = 1.0 / l
        qx = fx * l
        qy = fy * l
        qz = fz * l
        qw = fw * l
    end
    return qx,qy,qz,qw
end


function dotVector3(f::Vector3, g::Vector3)
    return (f.x * g.x) + (f.y * g.y) + (f.z * g.z)
end

function Base.show(io::IO, v::Vector3)
    print(io, "x:$(v.x), y:$(v.y), z:$(v.z)")
end

function Base.show(io::IO, v::Quaternion)
    print(io, "x:$(v.x), y:$(v.y), z:$(v.z), w:$(v.z)")
end

Base.Broadcast.broadcastable(q::Vector3) = Ref(q)
#####################################################
function doTimeStep!(metavoxel,dt,currentTimeStep)
    # update forces: go through edges, get currentposition from nodes, calc pos2 and update stresses and interior forces of nodes
    run_updateEdges!(
        metavoxel["E_sourceGPU"], 
        metavoxel["E_targetGPU"],
        metavoxel["E_areaGPU"],
        metavoxel["E_densityGPU"],
        metavoxel["E_stiffnessGPU"],
        metavoxel["E_stressGPU"],
        metavoxel["E_axisGPU"],
        metavoxel["E_currentRestLengthGPU"],
        metavoxel["E_pos2GPU"],
        metavoxel["E_angle1vGPU"],
        metavoxel["E_angle2vGPU"],
        metavoxel["E_angle1GPU"],
        metavoxel["E_angle2GPU"],
        metavoxel["E_intForce1GPU"],
        metavoxel["E_intMoment1GPU"],
        metavoxel["E_intForce2GPU"],
        metavoxel["E_intMoment2GPU"],
        metavoxel["E_dampGPU"],
        metavoxel["N_currPositionGPU"],
        metavoxel["N_orientGPU"])
    
    # update forces: go through nodes and update interior force (according to int forces from edges), integrate and update currpos
    run_updateNodes!(dt,currentTimeStep,
        metavoxel["N_positionGPU"], 
        metavoxel["N_restrainedGPU"],
        metavoxel["N_displacementGPU"],
        metavoxel["N_angleGPU"],
        metavoxel["N_currPositionGPU"],
        metavoxel["N_linMomGPU"],
        metavoxel["N_angMomGPU"],
        metavoxel["N_intForceGPU"],
        metavoxel["N_intMomentGPU"],
        metavoxel["N_forceGPU"],
        metavoxel["N_momentGPU"],
        metavoxel["N_orientGPU"],
        metavoxel["N_edgeIDGPU"], 
        metavoxel["N_edgeFirstGPU"], 
        metavoxel["E_intForce1GPU"],