Dynamic Drop Test Optimization.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "using LinearAlgebra\n",
    "using Plots\n",
    "import JSON\n",
    "using StaticArrays, BenchmarkTools\n",
    "using Base.Threads\n",
    "using CUDAnative\n",
    "using CuArrays,CUDAdrv \n",
    "# using Test\n",
    "import Base: +, * , -, ^"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "struct Vector3\n",
    "    x::Float64\n",
    "    y::Float64\n",
    "    z::Float64\n",
    "    function Vector3()\n",
    "        x=0.0\n",
    "        y=0.0\n",
    "        z=0.0\n",
    "        new(x,y,z)\n",
    "    end\n",
    "    function Vector3(x,y,z)\n",
    "       new(x,y,z)\n",
    "    end\n",
    "end\n",
    "struct Quaternion\n",
    "    x::Float64\n",
    "    y::Float64\n",
    "    z::Float64\n",
    "    w::Float64\n",
    "    function Quaternion()\n",
    "        x=0.0\n",
    "        y=0.0\n",
    "        z=0.0\n",
    "        w=1.0\n",
    "        new(x,y,z,w)\n",
    "    end\n",
    "    function Quaternion(x,y,z,w)\n",
    "        new(x,y,z,w)\n",
    "    end\n",
    "end\n",
    "struct RotationMatrix\n",
    "    te1::Float64\n",
    "    te2::Float64\n",
    "    te3::Float64\n",
    "    te4::Float64\n",
    "    te5::Float64\n",
    "    te6::Float64\n",
    "    te7::Float64\n",
    "    te8::Float64\n",
    "    te9::Float64\n",
    "    te10::Float64\n",
    "    te11::Float64\n",
    "    te12::Float64\n",
    "    te13::Float64\n",
    "    te14::Float64\n",
    "    te15::Float64\n",
    "    te16::Float64\n",
    "    function RotationMatrix()\n",
    "        te1 =0.0\n",
    "        te2 =0.0\n",
    "        te3 =0.0\n",
    "        te4 =0.0\n",
    "        te5 =0.0\n",
    "        te6 =0.0\n",
    "        te7 =0.0\n",
    "        te8 =0.0\n",
    "        te9 =0.0\n",
    "        te10=0.0\n",
    "        te11=0.0\n",
    "        te12=0.0\n",
    "        te13=0.0\n",
    "        te14=0.0\n",
    "        te15=0.0\n",
    "        te16=0.0\n",
    "        new(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)\n",
    "    end\n",
    "    function RotationMatrix(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)\n",
    "        new(te1,te2,te3,te4,te5,te6,te7,te8,te9,te10,te11,te12,te13,te14,te15,te16)\n",
    "    end\n",
    "end\n",
    "\n",
    "+(f::Vector3, g::Vector3)=Vector3(f.x+g.x , f.y+g.y,f.z+g.z )\n",
    "-(f::Vector3, g::Vector3)=Vector3(f.x-g.x , f.y-g.y,f.z-g.z )\n",
    "*(f::Vector3, g::Vector3)=Vector3(f.x*g.x , f.y*g.y,f.z*g.z )\n",
    "\n",
    "+(f::Vector3, g::Number)=Vector3(f.x+g , f.y+g,f.z+g )\n",
    "-(f::Vector3, g::Number)=Vector3(f.x-g , f.y-g,f.z-g )\n",
    "*(f::Vector3, g::Number)=Vector3(f.x*g , f.y*g,f.z*g )\n",
    "\n",
    "+(g::Vector3, f::Number)=Vector3(f.x+g , f.y+g,f.z+g )\n",
    "-(g::Vector3, f::Number)=Vector3(g-f.x , g-f.y,g-f.z )\n",
    "*(g::Vector3, f::Number)=Vector3(f.x*g , f.y*g,f.z*g )\n",
    "\n",
    "addX(f::Vector3, g::Number)=Vector3(f.x+g , f.y,f.z)\n",
    "addY(f::Vector3, g::Number)=Vector3(f.x , f.y+g,f.z )\n",
    "addZ(f::Vector3, g::Number)=Vector3(f.x , f.y,f.z+g )\n",
    "\n",
    "function normalizeVector3(f::Vector3)\n",
    "    leng=sqrt((f.x * f.x) + (f.y * f.y) + (f.z * f.z))\n",
    "    return Vector3(f.x/leng,f.y/leng,f.z/leng)\n",
    "    \n",
    "end\n",
    "function normalizeQuaternion(f::Quaternion)\n",
    "    l = sqrt((f.x * f.x) + (f.y * f.y) + (f.z * f.z)+ (f.w * f.w))\n",
    "    if l === 0 \n",
    "        qx = 0\n",
    "        qy = 0\n",
    "        qz = 0\n",
    "        qw = 1\n",
    "    else \n",
    "        l = 1 / l\n",
    "        qx = f.x * l\n",
    "        qy = f.y * l\n",
    "        qz = f.z * l\n",
    "        qw = f.w * l\n",
    "    end\n",
    "    return Quaternion(qx,qy,qz,qw)\n",
    "end\n",
    "\n",
    "function normalizeQuaternion1!(fx::Float64,fy::Float64,fz::Float64,fw::Float64)\n",
    "    l = sqrt((fx * fx) + (fy * fy) + (fz * fz)+ (fw * fw))\n",
    "    if l === 0 \n",
    "        qx = 0.0\n",
    "        qy = 0.0\n",
    "        qz = 0.0\n",
    "        qw = 1.0\n",
    "    else \n",
    "        l = 1.0 / l\n",
    "        qx = fx * l\n",
    "        qy = fy * l\n",
    "        qz = fz * l\n",
    "        qw = fw * l\n",
    "    end\n",
    "    return qx,qy,qz,qw\n",
    "end\n",
    "\n",
    "\n",
    "function dotVector3(f::Vector3, g::Vector3)\n",
    "    return (f.x * g.x) + (f.y * g.y) + (f.z * g.z)\n",
    "end\n",
    "\n",
    "function Base.show(io::IO, v::Vector3)\n",
    "    print(io, \"x:$(v.x), y:$(v.y), z:$(v.z)\")\n",
    "end\n",
    "\n",
    "function Base.show(io::IO, v::Quaternion)\n",
    "    print(io, \"x:$(v.x), y:$(v.y), z:$(v.z), w:$(v.z)\")\n",
    "end\n",
    "\n",
    "Base.Broadcast.broadcastable(q::Vector3) = Ref(q)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "struct material\n",
    "    E::Float64\n",
    "    mass::Float64\n",
    "    nu::Float64\n",
    "    rho::Float64\n",
    "    b::Float64\n",
    "    h::Float64\n",
    "    L::Float64\n",
    "    area::Float64\n",
    "    I::Float64\n",
    "    J::Float64\n",
    "    G::Float64\n",
    "    a1::Float64\n",
    "    a2::Float64\n",
    "    b1::Float64\n",
    "    b2::Float64\n",
    "    b3::Float64\n",
    "    massInverse::Float64\n",
    "    momentInertiaInverse::Float64\n",
    "    inertia::Float64\n",
    "    zeta::Float64\n",
    "    zetaCollision::Float64\n",
    "    muStatic::Float64\n",
    "    muKinetic::Float64\n",
    "    nomSize::Float64\n",
    "    sqA1::Float64\n",
    "    sqA2xIp::Float64 \n",
    "    sqB1::Float64\n",
    "    sqB2xFMp::Float64\n",
    "    sqB3xIp::Float64\n",
    "    _2xSqMxExS::Float64\n",
    "    function material()\n",
    "        E=2000\n",
    "        mass=10\n",
    "        nu=0.35\n",
    "        rho=7.85e-9\n",
    "        b=2.38\n",
    "        h=2.38\n",
    "        L=75\n",
    "        area=b*h\n",
    "        I=b*h^3/12\n",
    "        J=b*h*(b*b+h*h)/12\n",
    "        G = E * 1 / 3\n",
    "        a1=E*b*h/L\n",
    "        a2=G*J/L\n",
    "        b1=12*E*I/(L^3)\n",
    "        b2=6*E*I/(L^2)\n",
    "        b3=2*E*I/(L)\n",
    "        massInverse=1.0/mass\n",
    "        momentInertiaInverse=1.92e-6\n",
    "        inertia=1/momentInertiaInverse\n",
    "        zeta=1.0\n",
    "        zetaCollision=0.0\n",
    "        muStatic= 2.0\n",
    "        muKinetic= 0.1\n",
    "        nomSize=1.0\n",
    "        sqA1=sqrt(a1) \n",
    "        sqA2xIp=sqrt(a2*L*L/6.0) \n",
    "        sqB1=sqrt(b1) \n",
    "        sqB2xFMp=sqrt(b2*L/2) \n",
    "        sqB3xIp=sqrt(b3*L*L/6.0)\n",
    "        _2xSqMxExS = (2.0*sqrt(mass*E*nomSize))\n",
    "        new(E,mass,nu,rho,b,h,L,area,I,J,G,a1,a2,b1,b2,b3,massInverse,momentInertiaInverse,inertia,zeta,zetaCollision,muStatic,muKinetic,nomSize,sqA1,sqA2xIp,sqB1,sqB2xFMp,sqB3xIp,_2xSqMxExS)\n",
    "    end\n",
    "    function material(E,mass,nu,rho,b,h,L,momentInertiaInverse,zeta,zetaCollision,muStatic,muKinetic,nomSize)\n",
    "        area=b*h\n",
    "        I=b*h^3/12\n",
    "        J=b*h*(b*b+h*h)/12\n",
    "        G = E * 1 / 3\n",
    "        a1=E*b*h/L\n",
    "        a2=G*J/L\n",
    "        b1=12*E*I/(L^3)\n",
    "        b2=6*E*I/(L^2)\n",
    "        b3=2*E*I/(L)\n",
    "        massInverse=1.0/mass\n",
    "        inertia=1/momentInertiaInverse\n",
    "        sqA1=sqrt(a1) \n",
    "        sqA2xIp=sqrt(a2*L*L/6.0) \n",
    "        sqB1=sqrt(b1) \n",
    "        sqB2xFMp=sqrt(b2*L/2) \n",
    "        sqB3xIp=sqrt(b3*L*L/6.0)\n",
    "        _2xSqMxExS = (2.0*sqrt(mass*E*nomSize))\n",
    "        new(E,mass,nu,rho,b,h,L,area,I,J,G,a1,a2,b1,b2,b3,massInverse,momentInertiaInverse,inertia,zeta,zetaCollision,muStatic,muKinetic,nomSize,sqA1,sqA2xIp,sqB1,sqB2xFMp,sqB3xIp,_2xSqMxExS)\n",
    "    end\n",
    "end\n",
    "\n",
    "struct voxelMaterial\n",
    "    E::Float64\n",
    "    mass::Float64\n",
    "    nu::Float64\n",
    "    rho::Float64\n",
    "    massInverse::Float64\n",
    "    momentInertiaInverse::Float64\n",
    "    inertia::Float64\n",
    "    zeta::Float64\n",
    "    zetaCollision::Float64\n",
    "    muStatic::Float64\n",
    "    muKinetic::Float64\n",
    "    nomSize::Float64\n",
    "    _2xSqMxExS::Float64\n",
    "    function voxelMaterial()\n",
    "        E=2000\n",
    "        mass=10\n",
    "        nu=0.35\n",
    "        rho=7.85e-9\n",
    "        massInverse=1.0/mass\n",
    "        momentInertiaInverse=1.92e-6\n",
    "        inertia=1/momentInertiaInverse\n",
    "        zeta=1.0\n",
    "        zetaCollision=0.0\n",
    "        muStatic= 2.0\n",
    "        muKinetic= 0.1\n",
    "        nomSize=1.0\n",
    "        _2xSqMxExS = (2.0*sqrt(mass*E*nomSize))\n",
    "        new(E,mass,nu,rho,massInverse,momentInertiaInverse,inertia,zeta,zetaCollision,muStatic,muKinetic,nomSize,_2xSqMxExS)\n",
    "    end\n",
    "    function voxelMaterial(E,mass,nu,rho,momentInertiaInverse,zeta,zetaCollision,muStatic,muKinetic,nomSize)\n",
    "        massInverse=1.0/mass\n",
    "        inertia=1/momentInertiaInverse\n",
    "        _2xSqMxExS = (2.0*sqrt(mass*E*nomSize))\n",
    "        new(E,mass,nu,rho,massInverse,momentInertiaInverse,inertia,zeta,zetaCollision,muStatic,muKinetic,nomSize,_2xSqMxExS)\n",
    "    end\n",
    "end\n",
    "\n",
    "struct edgeMaterial\n",
    "    E::Float64\n",
    "    nu::Float64\n",
    "    rho::Float64\n",
    "    b::Float64\n",
    "    h::Float64\n",
    "    L::Float64\n",
    "    area::Float64\n",
    "    I::Float64\n",
    "    J::Float64\n",
    "    G::Float64\n",
    "    a1::Float64\n",
    "    a2::Float64\n",
    "    b1::Float64\n",
    "    b2::Float64\n",
    "    b3::Float64\n",
    "    sqA1::Float64\n",
    "    sqA2xIp::Float64 \n",
    "    sqB1::Float64\n",
    "    sqB2xFMp::Float64\n",
    "    sqB3xIp::Float64\n",
    "    dampingM::Float64\n",
    "    function edgeMaterial()\n",
    "        E=2000\n",
    "        mass=10.0\n",
    "        nu=0.35\n",
    "        rho=7.85e-9\n",
    "        b=2.38\n",
    "        h=2.38\n",
    "        L=75\n",
    "        area=b*h\n",
    "        I=b*h^3/12\n",
    "        J=b*h*(b*b+h*h)/12\n",
    "        G = E * 1 / 3\n",
    "        a1=E*b*h/L\n",
    "        a2=G*J/L\n",
    "        b1=12*E*I/(L^3)\n",
    "        b2=6*E*I/(L^2)\n",
    "        b3=2*E*I/(L)\n",
    "        sqA1=sqrt(a1) \n",
    "        sqA2xIp=sqrt(a2*L*L/6.0) \n",
    "        sqB1=sqrt(b1) \n",
    "        sqB2xFMp=sqrt(b2*L/2) \n",
    "        sqB3xIp=sqrt(b3*L*L/6.0)\n",
    "        zeta=1\n",
    "        dampingM=2.0*sqrt(mass)*zeta\n",
    "        new(E,nu,rho,b,h,L,area,I,J,G,a1,a2,b1,b2,b3,sqA1,sqA2xIp,sqB1,sqB2xFMp,sqB3xIp,dampingM)\n",
    "    end\n",
    "    function edgeMaterial(E,mass,nu,rho,b,h,L)\n",
    "        area=b*h\n",
    "        I=b*h^3/12\n",
    "        J=b*h*(b*b+h*h)/12\n",
    "        G = E * 1 / 3\n",
    "        a1=E*b*h/L\n",
    "        a2=G*J/L\n",
    "        b1=12*E*I/(L^3)\n",
    "        b2=6*E*I/(L^2)\n",
    "        b3=2*E*I/(L)\n",
    "        sqA1=sqrt(a1) \n",
    "        sqA2xIp=sqrt(a2*L*L/6.0) \n",
    "        sqB1=sqrt(b1) \n",
    "        sqB2xFMp=sqrt(b2*L/2) \n",
    "        sqB3xIp=sqrt(b3*L*L/6.0)\n",
    "        zeta=1\n",
    "        dampingM=2.0*sqrt(mass)*zeta\n",
    "        new(E,nu,rho,b,h,L,area,I,J,G,a1,a2,b1,b2,b3,sqA1,sqA2xIp,sqB1,sqB2xFMp,sqB3xIp,dampingM)\n",
    "    end\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "doTimeStep! (generic function with 1 method)"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function doTimeStep!(metavoxel,dt,currentTimeStep)\n",
    "    # update forces: go through edges, get currentposition from nodes, calc pos2 and update stresses and interior forces of nodes\n",
    "    run_updateEdges!(dt,\n",
    "        metavoxel[\"E_sourceGPU\"], \n",
    "        metavoxel[\"E_targetGPU\"],\n",
    "        metavoxel[\"E_areaGPU\"],\n",
    "        metavoxel[\"E_densityGPU\"],\n",
    "        metavoxel[\"E_stiffnessGPU\"],\n",
    "        metavoxel[\"E_stressGPU\"],\n",
    "        metavoxel[\"E_axisGPU\"],\n",
    "        metavoxel[\"E_currentRestLengthGPU\"],\n",
    "        metavoxel[\"E_pos2GPU\"],\n",
    "        metavoxel[\"E_angle1vGPU\"],\n",
    "        metavoxel[\"E_angle2vGPU\"],\n",
    "        metavoxel[\"E_angle1GPU\"],\n",
    "        metavoxel[\"E_angle2GPU\"],\n",
    "        metavoxel[\"E_intForce1GPU\"],\n",
    "        metavoxel[\"E_intMoment1GPU\"],\n",
    "        metavoxel[\"E_intForce2GPU\"],\n",
    "        metavoxel[\"E_intMoment2GPU\"],\n",
    "        metavoxel[\"E_dampGPU\"],\n",
    "        metavoxel[\"E_materialGPU\"],\n",
    "        metavoxel[\"N_currPositionGPU\"],\n",
    "        metavoxel[\"N_orientGPU\"])\n",
    "    \n",
    "    # update forces: go through nodes and update interior force (according to int forces from edges), integrate and update currpos\n",
    "    run_updateNodes!(dt,currentTimeStep,\n",
    "        metavoxel[\"N_positionGPU\"], \n",
    "        metavoxel[\"N_restrainedGPU\"],\n",
    "        metavoxel[\"N_displacementGPU\"],\n",
    "        metavoxel[\"N_angleGPU\"],\n",
    "        metavoxel[\"N_currPositionGPU\"],\n",
    "        metavoxel[\"N_linMomGPU\"],\n",
    "        metavoxel[\"N_angMomGPU\"],\n",
    "        metavoxel[\"N_intForceGPU\"],\n",
    "        metavoxel[\"N_intMomentGPU\"],\n",
    "        metavoxel[\"N_forceGPU\"],\n",
    "        metavoxel[\"N_momentGPU\"],\n",
    "        metavoxel[\"N_orientGPU\"],\n",
    "        metavoxel[\"N_edgeIDGPU\"], \n",
    "        metavoxel[\"N_edgeFirstGPU\"],\n",
    "        metavoxel[\"N_materialGPU\"], \n",
    "        metavoxel[\"E_intForce1GPU\"],\n",
    "        metavoxel[\"E_intMoment1GPU\"],\n",
    "        metavoxel[\"E_intForce2GPU\"],\n",
    "        metavoxel[\"E_intMoment2GPU\"])\n",
    "    \n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "run_updateEdges! (generic function with 1 method)"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function updateEdges!(dt,E_source,E_target,E_area,E_density,E_stiffness,E_stress,E_axis,\n",
    "        E_currentRestLength,E_pos2,E_angle1v,E_angle2v,\n",
    "        E_angle1,E_angle2,E_intForce1,E_intMoment1,E_intForce2,E_intMoment2,E_damp,E_material,\n",
    "        N_currPosition,N_orient)\n",
    "\n",
    "    index = (blockIdx().x - 1) * blockDim().x + threadIdx().x\n",
    "    stride = blockDim().x * gridDim().x\n",
    "    ## @cuprintln(\"thread $index, block $stride\")\n",
    "    N=length(E_source)\n",
    "    for i = index:stride:N\n",
    "        \n",
    "        @inbounds pVNeg=N_currPosition[E_source[i]]\n",
    "        @inbounds pVPos=N_currPosition[E_target[i]]\n",
    "        \n",
    "        @inbounds oVNeg=N_orient[E_source[i]]\n",
    "        @inbounds oVPos=N_orient[E_target[i]]\n",
    "        \n",
    "        @inbounds oldPos2=Vector3(E_pos2[i].x,E_pos2[i].y,E_pos2[i].z) #?copy?\n",
    "        @inbounds oldAngle1v = Vector3(E_angle1v[i].x,E_angle1v[i].y,E_angle1v[i].z)\n",
    "        @inbounds oldAngle2v = Vector3(E_angle2v[i].x,E_angle2v[i].y,E_angle2v[i].z)# remember the positions/angles from last timestep to calculate velocity\n",
    "        \n",
    "        \n",
    "        @inbounds E_pos2[i],E_angle1v[i],E_angle2v[i],E_angle1[i],E_angle2[i],totalRot= orientLink!(E_currentRestLength[i],pVNeg,pVPos,oVNeg,oVPos,E_axis[i])\n",
    "        \n",
    "        @inbounds dPos2   = Vector3(0.5,0.5,0.5) * (E_pos2[i]-oldPos2)  #deltas for local damping. velocity at center is half the total velocity\n",
    "        @inbounds dAngle1 = Vector3(0.5,0.5,0.5) *(E_angle1v[i]-oldAngle1v)\n",
    "        @inbounds dAngle2 = Vector3(0.5,0.5,0.5) *(E_angle2v[i]-oldAngle2v)\n",
    "        \n",
    "        \n",
    "        @inbounds strain=(E_pos2[i].x/E_currentRestLength[i])\n",
    "        \n",
    "        positiveEnd=true\n",
    "        if axialStrain( positiveEnd,strain)>100.0\n",
    "            diverged=true\n",
    "            @cuprintln(\"DIVERGED!!!!!!!!!!\")\n",
    "            return \n",
    "        end\n",
    "        \n",
    "        @inbounds E = E_stiffness[i]\n",
    "        \n",
    "        \n",
    "        \n",
    "        @inbounds l   = E_currentRestLength[i]\n",
    "        \n",
    "\n",
    "        \n",
    "        @inbounds nu = convert(Float64,E_material[i].nu)\n",
    " \n",
    "        # Cross Section inputs, must be floats        \n",
    "        @inbounds E = convert(Float64,E_material[i].E)  # MPa\n",
    "        @inbounds h = convert(Float64,E_material[i].h)  # mm\n",
    "        @inbounds b = convert(Float64,E_material[i].b) # mm\n",
    "        \n",
    "\n",
    "        @inbounds a1=convert(Float64,E_material[i].a1)\n",
    "        @inbounds a2=convert(Float64,E_material[i].a2)\n",
    "        @inbounds b1=convert(Float64,E_material[i].b1)\n",
    "        @inbounds b2=convert(Float64,E_material[i].b2)\n",
    "        @inbounds b3=convert(Float64,E_material[i].b3)\n",
    "        \n",
    "        \n",
    "\n",
    "        \n",
    "        #inbounds currentTransverseArea=25.0 #?? change!!!!!! E_area[i]\n",
    "        @inbounds currentTransverseArea= b*h\n",
    "        @inbounds _stress=updateStrain(strain,E)\n",
    "        \n",
    "        #@inbounds currentTransverseArea= E_area[i]\n",
    "        #@inbounds _stress=updateStrain(strain,E_stiffness[i])\n",
    "        \n",
    "        @inbounds E_stress[i]=_stress\n",
    "        \n",
    "        #@cuprintln(\"_stress $_stress\")\n",
    "        x=(_stress*currentTransverseArea)\n",
    "        @inbounds y=(b1*E_pos2[i].y-b2*(E_angle1v[i].z + E_angle2v[i].z))\n",
    "        @inbounds z=(b1*E_pos2[i].z + b2*(E_angle1v[i].y + E_angle2v[i].y))\n",
    "        \n",
    "        x=convert(Float64,x)\n",
    "        y=convert(Float64,y)\n",
    "        z=convert(Float64,z)\n",
    "        \n",
    "        # Use Curstress instead of -a1*Pos2.x to account for non-linear deformation \n",
    "        forceNeg = Vector3(x,y,z)\n",
    "        \n",
    "        forcePos = Vector3(-x,-y,-z)\n",
    "        \n",
    "        @inbounds x= (a2*(E_angle2v[i].x-E_angle1v[i].x))\n",
    "        @inbounds y= (-b2*E_pos2[i].z-b3*(2.0*E_angle1v[i].y+E_angle2v[i].y))\n",
    "        @inbounds z=(b2*E_pos2[i].y - b3*(2.0*E_angle1v[i].z + E_angle2v[i].z))  \n",
    "        x=convert(Float64,x)\n",
    "        y=convert(Float64,y)\n",
    "        z=convert(Float64,z)\n",
    "        momentNeg = Vector3(x,y,z)\n",
    "        \n",
    "\n",
    "        @inbounds x= (a2*(E_angle1v[i].x-E_angle2v[i].x))\n",
    "        @inbounds y= (-b2*E_pos2[i].z- b3*(E_angle1v[i].y+2.0*E_angle2v[i].y))\n",
    "        @inbounds z=(b2*E_pos2[i].y - b3*(E_angle1v[i].z + 2.0*E_angle2v[i].z))\n",
    "        x=convert(Float64,x)\n",
    "        y=convert(Float64,y)\n",
    "        z=convert(Float64,z)\n",
    "        momentPos = Vector3(x,y,z)\n",
    "        \n",
    "        \n",
    "        ### damping\n",
    "        @inbounds if E_damp[i] #first pass no damping\n",
    "            @inbounds sqA1     =convert(Float64,E_material[i].sqA1)\n",
    "            @inbounds sqA2xIp  =convert(Float64,E_material[i].sqA2xIp)\n",
    "            @inbounds sqB1     =convert(Float64,E_material[i].sqB1)\n",
    "            @inbounds sqB2xFMp =convert(Float64,E_material[i].sqB2xFMp)\n",
    "            @inbounds sqB3xIp  =convert(Float64,E_material[i].sqB3xIp)\n",
    "            \n",
    "            dampingMultiplier=Vector3(28099.3,28099.3,28099.3) # 2*mat->_sqrtMass*mat->zetaInternal/previousDt;?? todo link to material\n",
    "            \n",
    "            zeta=1\n",
    "            dampingM= convert(Float64,E_material[i].dampingM)/dt\n",
    "            dampingMultiplier=Vector3(dampingM,dampingM,dampingM)\n",
    "            \n",
    "            posCalc=Vector3(sqA1*dPos2.x, sqB1*dPos2.y - sqB2xFMp*(dAngle1.z+dAngle2.z),sqB1*dPos2.z + sqB2xFMp*(dAngle1.y+dAngle2.y))\n",
    "            \n",
    "            \n",
    "            forceNeg =forceNeg + (dampingMultiplier*posCalc);\n",
    "            forcePos =forcePos - (dampingMultiplier*posCalc);\n",
    "\n",
    "            momentNeg -= Vector3(0.5,0.5,0.5)*dampingMultiplier*Vector3(-sqA2xIp*(dAngle2.x - dAngle1.x),\n",
    "                                                                    sqB2xFMp*dPos2.z + sqB3xIp*(2*dAngle1.y + dAngle2.y),\n",
    "                                                                    -sqB2xFMp*dPos2.y + sqB3xIp*(2*dAngle1.z + dAngle2.z));\n",
    "            momentPos -= Vector3(0.5,0.5,0.5)*dampingMultiplier*Vector3(sqA2xIp*(dAngle2.x - dAngle1.x),\n",
    "                                                                sqB2xFMp*dPos2.z + sqB3xIp*(dAngle1.y + 2*dAngle2.y),\n",
    "                                                                -sqB2xFMp*dPos2.y + sqB3xIp*(dAngle1.z + 2*dAngle2.z));\n",
    "\n",
    "        else\n",
    "           @inbounds E_damp[i]=true \n",
    "        end\n",
    "\n",
    "        smallAngle=true\n",
    "        if !smallAngle # ?? check\n",
    "            @inbounds forceNeg = RotateVec3DInv(E_angle1[i],forceNeg)\n",
    "            @inbounds momentNeg = RotateVec3DInv(E_angle1[i],momentNeg)\n",
    "        end\n",
    "        \n",
    "        @inbounds forcePos = RotateVec3DInv(E_angle2[i],forcePos)\n",
    "        @inbounds momentPos = RotateVec3DInv(E_angle2[i],momentPos)\n",
    "\n",
    "        @inbounds forceNeg =toAxisOriginalVector3(forceNeg,E_axis[i])\n",
    "        @inbounds forcePos =toAxisOriginalVector3(forcePos,E_axis[i])\n",
    "\n",
    "        @inbounds momentNeg=toAxisOriginalQuat(momentNeg,E_axis[i])# TODOO CHECKKKKKK\n",
    "        @inbounds momentPos=toAxisOriginalQuat(momentPos,E_axis[i])\n",
    "\n",
    "\n",
    "        @inbounds E_intForce1[i] =forceNeg\n",
    "        @inbounds E_intForce2[i] =forcePos\n",
    "        \n",
    "\n",
    "\n",
    "        @inbounds x= momentNeg.x\n",
    "        @inbounds y= momentNeg.y\n",
    "        @inbounds z= momentNeg.z  \n",
    "        x=convert(Float64,x)\n",
    "        y=convert(Float64,y)\n",
    "        z=convert(Float64,z)\n",
    "        \n",
    "        @inbounds E_intMoment1[i]=Vector3(x,y,z)\n",
    "\n",
    "        @inbounds x= momentNeg.x\n",
    "        @inbounds y= momentNeg.y\n",
    "        @inbounds z= momentNeg.z\n",
    "        x=convert(Float64,x)\n",
    "        y=convert(Float64,y)\n",
    "        z=convert(Float64,z)\n",
    "        \n",
    "        @inbounds E_intMoment2[i]=Vector3(x,y,z)\n",
    "        \n",
    "        #x=E_pos2[i].x*10000000000\n",
    "        #y=E_pos2[i].y*10000000000\n",
    "        #z=E_pos2[i].z*10000000000\n",
    "        #@cuprintln(\"pos2 x $x, y $y, z $z \")\n",
    "        ##x=E_intMoment2[i].x*10000000000\n",
    "        #y=E_intMoment2[i].y*10000000000\n",
    "        #z=E_intMoment2[i].z*10000000000\n",
    "        #@cuprintln(\"E_intMoment2 x $x, y $y, z $z \")\n",
    "\n",
    "        \n",
    "        \n",
    "    end\n",
    "    return\n",
    "end\n",
    "\n",
    "function run_updateEdges!(dt,E_source,E_target,E_area,E_density,E_stiffness,\n",
    "        E_stress,E_axis,E_currentRestLength,E_pos2,E_angle1v,E_angle2v,\n",
    "        E_angle1,E_angle2,E_intForce1,E_intMoment1,E_intForce2,E_intMoment2,\n",
    "        E_damp,E_material,N_currPosition,N_orient)\n",
    "    N=length(E_source)\n",
    "    numblocks = ceil(Int, N/256)\n",
    "    CuArrays.@sync begin\n",
    "        @cuda threads=256 blocks=numblocks updateEdges!(dt,E_source,E_target,E_area,E_density,\n",
    "            E_stiffness,E_stress,E_axis,E_currentRestLength,E_pos2,E_angle1v,\n",
    "            E_angle2v,E_angle1,E_angle2,E_intForce1,E_intMoment1,E_intForce2,\n",
    "            E_intMoment2,E_damp,E_material,N_currPosition,N_orient)\n",
    "    end\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 23,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "floorForce! (generic function with 1 method)"
      ]
     },
     "execution_count": 23,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function floorPenetration(y)\n",
    "    floor=0.0\n",
    "    p=0.0\n",
    "    if(y<floor)\n",
    "        p=floor-y\n",
    "    end\n",
    "    return p\n",
    "end\n",
    "#Returns the interference (in meters) between the collision envelope of this voxel and the floor at Z=0. Positive numbers correspond to interference. If the voxel is not touching the floor 0 is returned.\n",
    "\n",
    "function penetrationStiffness(E,nomSize)\n",
    "    return (2.0*E*nomSize)\n",
    "end \n",
    "#!< returns the stiffness with which this voxel will resist penetration. This is calculated according to E*A/L with L = voxelSize/2.\n",
    "\n",
    "\n",
    "function globalDampingTranslateC(_2xSqMxExS,zetaGlobal)\n",
    "    return zetaGlobal*_2xSqMxExS;\n",
    "end #!< Returns the global material damping coefficient (translation)\n",
    "\n",
    "function collisionDampingTranslateC(_2xSqMxExS,zetaCollision)\n",
    "#     _2xSqIxExSxSxS = (2.0f*CUDAnative.sqrt(_momentInertia*E*nomSize*nomSize*nomSize));\n",
    "    return zetaCollision*_2xSqMxExS;\n",
    "end #!< Returns the global material damping coefficient (translation)\n",
    "\n",
    "function floorForce!(dt,pTotalForce,y,linMom,FloorStaticFriction,N_material)\n",
    "    E=convert(Float64,N_material.E)\n",
    "    nomSize=convert(Float64,N_material.nomSize)\n",
    "    mass=convert(Float64,N_material.mass)\n",
    "    massInverse=convert(Float64,N_material.massInverse)\n",
    "    muStatic=convert(Float64,N_material.muStatic)\n",
    "    muKinetic=convert(Float64,N_material.muKinetic)\n",
    "    _2xSqMxExS=convert(Float64,N_material._2xSqMxExS)\n",
    "    zetaCollision=convert(Float64,N_material.zetaCollision)\n",
    "    \n",
    "    CurPenetration = floorPenetration(convert(Float64,y)); #for now use the average.\n",
    "\n",
    "\n",
    "    if (CurPenetration>=0.0)\n",
    "        vel = linMom*Vector3((massInverse),(massInverse),(massInverse)) #Returns the 3D velocity of this voxel in m/s (GCS)\n",
    "        horizontalVel= Vector3(convert(Float64,vel.x), 0.0, convert(Float64,vel.z));\n",
    "        \n",
    "        normalForce = penetrationStiffness(E,nomSize)*CurPenetration;\n",
    "        pTotalForce=Vector3( pTotalForce.x, convert(Float64,pTotalForce.y) + normalForce - collisionDampingTranslateC(_2xSqMxExS,zetaCollision)*convert(Float64,vel.y),pTotalForce.z)\n",
    "        #in the z direction: k*x-C*v - spring and damping\n",
    "\n",
    "        if (FloorStaticFriction) #If this voxel is currently in static friction mode (no lateral motion) \n",
    "            # assert(horizontalVel.Length2() == 0);\n",
    "            surfaceForceSq = convert(Float64,(pTotalForce.x*pTotalForce.x + pTotalForce.z*pTotalForce.z)); #use squares to avoid a square root\n",
    "            frictionForceSq = (muStatic*normalForce)*(muStatic*normalForce);\n",
    "\n",
    "\n",
    "            if (surfaceForceSq > frictionForceSq) \n",
    "                FloorStaticFriction=false; #if we're breaking static friction, leave the forces as they currently have been calculated to initiate motion this time step\n",
    "            end\n",
    "\n",
    "        else  #even if we just transitioned don't process here or else with a complete lack of momentum it'll just go back to static friction\n",
    "            #add a friction force opposing velocity according to the normal force and the kinetic coefficient of friction\n",
    "            leng=CUDAnative.sqrt((convert(Float64,vel.x) * convert(Float64,vel.x)) + (0.0 * 0.0) + (convert(Float64,vel.z) * convert(Float64,vel.z)))\n",
    "            if(leng>0)\n",
    "                horizontalVel= Vector3(convert(Float64,vel.x)/(leng),0.0,convert(Float64,vel.z)/(leng))\n",
    "            else\n",
    "                horizontalVel= Vector3(convert(Float64,vel.x)*(leng),0.0,convert(Float64,vel.z)*(leng))\n",
    "\n",
    "            end   \n",
    "            pTotalForce = pTotalForce- Vector3(muKinetic*normalForce,muKinetic*normalForce,muKinetic*normalForce) * horizontalVel;\n",
    "        end\n",
    "\n",
    "    else \n",
    "        FloorStaticFriction=false;\n",
    "    end\n",
    "    \n",
    "    \n",
    "    \n",
    "    return pTotalForce,FloorStaticFriction\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 24,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "run_updateNodes! (generic function with 1 method)"
      ]
     },
     "execution_count": 24,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function updateNodes!(dt,currentTimeStep,N_position, N_restrained,N_displacement,N_angle,N_currPosition,N_linMom,N_angMom,\n",
    "        N_intForce,N_intMoment,N_force,N_moment,N_orient,N_edgeID,N_edgeFirst,N_material,\n",
    "        E_intForce1,E_intMoment1,E_intForce2,E_intMoment2)\n",
    "\n",
    "    index = (blockIdx().x - 1) * blockDim().x + threadIdx().x\n",
    "    stride = blockDim().x * gridDim().x\n",
    "    ## @cuprintln(\"thread $index, block $stride\")\n",
    "    N,M=size(N_edgeID)\n",
    "    for i = index:stride:N\n",
    "        if false\n",
    "         #@inbounds if N_restrained[i]\n",
    "            return\n",
    "        else\n",
    "            for j in 1:M\n",
    "                temp=N_edgeID[i,j]\n",
    "                @inbounds if (N_edgeID[i,j]!=-1)\n",
    "                    #@cuprintln(\"i $i, j $j, N_edgeID[i,j] $temp\")\n",
    "                    @inbounds N_intForce[i]=ifelse(N_edgeFirst[i,j], N_intForce[i]+E_intForce1[N_edgeID[i,j]], N_intForce[i]+E_intForce2[N_edgeID[i,j]] )\n",
    "                    @inbounds N_intMoment[i]=ifelse(N_edgeFirst[i,j], N_intMoment[i]+E_intMoment1[N_edgeID[i,j]], N_intMoment[i]+E_intMoment2[N_edgeID[i,j]] )\n",
    "                end\n",
    "            end\n",
    "            \n",
    "            isFloorEnabled=true;#todo make these arguments ??\n",
    "            static=false;#todo make these arguments ??\n",
    "            FloorStaticFriction=false; #todo make these arguments ??\n",
    "            \n",
    "            #get properties\n",
    "            \n",
    "            @inbounds E=N_material[i].E\n",
    "            @inbounds nomSize=N_material[i].nomSize\n",
    "            @inbounds mass=N_material[i].mass\n",
    "            @inbounds massInverse=N_material[i].massInverse\n",
    "            \n",
    "            @inbounds curForce = force(N_intForce[i],N_orient[i],N_force[i],static,currentTimeStep,mass)\n",
    "            \n",
    "            \n",
    "            \n",
    "            \n",
    "            fricForce = Vector3(curForce.x,curForce.y,curForce.z);\n",
    "            if (isFloorEnabled)\n",
    "                @inbounds curForce,FloorStaticFriction=floorForce!(dt,curForce,N_currPosition[i].y,\n",
    "                    Vector3(N_linMom[i].x,N_linMom[i].y ,N_linMom[i].z),FloorStaticFriction,N_material[i])\n",
    "                \n",
    "            end\n",
    "            \n",
    "            fricForce = curForce-fricForce;\n",
    "            \n",
    "            \n",
    "            #########################################\n",
    "            \n",
    "            @inbounds N_intForce[i]=Vector3(0,0,0)\n",
    "        \n",
    "            \n",
    "            @inbounds N_linMom[i]=N_linMom[i]+curForce*Vector3(dt,dt,dt) #todo make sure right\n",
    "            @inbounds translate=N_linMom[i]*Vector3((dt*massInverse),(dt*massInverse),(dt*massInverse)) # ??massInverse\n",
    "            \n",
    "            \n",
    "            ############################\n",
    "            \n",
    "            \n",
    "            #we need to check for friction conditions here (after calculating the translation) and stop things accordingly\n",
    "            @inbounds if (isFloorEnabled && floorPenetration( N_currPosition[i].y)>= 0.0 && !static)#we must catch a slowing voxel here since it all boils down to needing access to the dt of this timestep.\n",
    "                work =convert(Float64,fricForce.x*translate.x + fricForce.z*translate.z); #F dot disp\n",
    "                @inbounds hKe = 0.5*massInverse*convert(Float64,(N_linMom[i].x*N_linMom[i].x + N_linMom[i].z*N_linMom[i].z)); #horizontal kinetic energy\n",
    "\n",
    "                if((hKe + work) <= 0.0) \n",
    "                    FloorStaticFriction=true; #this checks for a change of direction according to the work-energy principle\n",
    "                end\n",
    "\n",
    "                if(FloorStaticFriction)\n",
    "                    #if we're in a state of static friction, zero out all horizontal motion\n",
    "                    N_linMom[i]=Vector3(0.0 ,convert(Float64,N_linMom[i].y) ,0.0)\n",
    "                    translate=Vector3(0.0 ,convert(Float64,translate.y) ,0.0)\n",
    "                end\n",
    "            else\n",
    "                FloorStaticFriction=false\n",
    "            end\n",
    "        \n",
    "            \n",
    "            @inbounds N_currPosition[i]=N_currPosition[i]+translate\n",
    "            @inbounds N_displacement[i]=N_displacement[i]+translate\n",
    "            \n",
    "            \n",
    "            \n",
    "            # Rotation\n",
    "            @inbounds curMoment = moment(N_intMoment[i],N_orient[i],N_moment[i]) \n",
    "            \n",
    "            \n",
    "            \n",
    "            @inbounds N_intMoment[i]=Vector3(0,0,0) # do i really need it?\n",
    "            \n",
    "            @inbounds N_angMom[i]=N_angMom[i]+curMoment*Vector3(dt,dt,dt)\n",
    "            \n",
    "            @inbounds momentInertiaInverse=N_material[i].momentInertiaInverse\n",
    "            \n",
    "            @inbounds temp=FromRotationVector(N_angMom[i]*Vector3((dt*momentInertiaInverse),(dt*momentInertiaInverse),(dt*momentInertiaInverse)))\n",
    "\n",
    "            \n",
    "            @inbounds N_orient[i]=multiplyQuaternions(temp,N_orient[i])\n",
    "            \n",
    "            \n",
    "        end\n",
    "    end\n",
    "    return\n",
    "end\n",
    "\n",
    "\n",
    "function run_updateNodes!(dt,currentTimeStep,N_position, N_restrained,N_displacement, N_angle,N_currPosition,\n",
    "        N_linMom,N_angMom,N_intForce,N_intMoment,N_force,N_moment,N_orient,N_edgeID,N_edgeFirst,N_material,\n",
    "        E_intForce1,E_intMoment1,E_intForce2,E_intMoment2)\n",
    "    N=length(N_intForce)\n",
    "    numblocks = ceil(Int, N/256)\n",
    "    CuArrays.@sync begin\n",
    "        @cuda threads=256 blocks=numblocks updateNodes!(dt,currentTimeStep,N_position, N_restrained,N_displacement, \n",
    "            N_angle,N_currPosition,N_linMom,N_angMom,N_intForce,N_intMoment,N_force,N_moment,N_orient,N_edgeID,N_edgeFirst,N_material,\n",
    "            E_intForce1,E_intMoment1,E_intForce2,E_intMoment2)\n",
    "    end\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "orientLink! (generic function with 1 method)"
      ]
     },
     "execution_count": 25,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function orientLink!(currentRestLength,pVNeg,pVPos,oVNeg,oVPos,axis)  # updates pos2, angle1, angle2, and smallAngle //Quat3D<double> /*double restLength*/\n",
    "        \n",
    "    pos2 = toAxisXVector3(pVPos-pVNeg,axis) # digit truncation happens here...\n",
    "    angle1 = toAxisXQuat(oVNeg,axis)\n",
    "    angle2 = toAxisXQuat(oVPos,axis)\n",
    "    \n",
    "    #x=pos2.x*10000000000\n",
    "    #y=pos2.y*10000000000\n",
    "    #z=pos2.z*10000000000\n",
    "    #@cuprintln(\"pos2 x $x, y $y, z $z \")\n",
    "    \n",
    "    \n",
    "    totalRot = conjugate(angle1) #keep track of the total rotation of this bond (after toAxisX()) # Quat3D<double>\n",
    "    pos2 = RotateVec3D(totalRot,pos2)\n",
    "    \n",
    "\n",
    "    angle2 = Quaternion(angle2.x*totalRot.x,angle2.y*totalRot.y,angle2.z*totalRot.z,angle2.w*totalRot.w)\n",
    "    angle1 = Quaternion(0.0,0.0,0.0,1.0)#new THREE.Quaternion() #zero for now...\n",
    "\n",
    "    smallAngle=true #todo later remove\n",
    "    \n",
    "    \n",
    "    if (smallAngle)\t #Align so Angle1 is all zeros\n",
    "        #pos2[1] =pos2[1]- currentRestLength #only valid for small angles\n",
    "        pos2=Vector3(pos2.x-currentRestLength,pos2.y,pos2.z)\n",
    "    else  #Large angle. Align so that Pos2.y, Pos2.z are zero.\n",
    "        # FromAngleToPosX(angle1,pos2) #get the angle to align Pos2 with the X axis\n",
    "        # totalRot = angle1.clone().multiply(totalRot)  #update our total rotation to reflect this\n",
    "        # angle2 = angle1.clone().multiply(  angle2) #rotate angle2\n",
    "        # pos2 = new THREE.Vector3(pos2.length() - currentRestLength, 0, 0);\n",
    "    end\n",
    "    \n",
    "    angle1v = ToRotationVector(angle1)\n",
    "    angle2v = ToRotationVector(angle2)\n",
    "    \n",
    "    \n",
    "    # pos2,angle1v,angle2v,angle1,angle2,\n",
    "    return pos2,angle1v,angle2v,angle1,angle2,totalRot\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "toAxisOriginalQuat (generic function with 1 method)"
      ]
     },
     "execution_count": 26,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function toAxisXVector3(pV::Vector3,axis::Vector3) #TODO CHANGE\n",
    "\n",
    "    xaxis=Vector3(1.0,0.0,0.0)\n",
    "    vector=normalizeVector3(axis)\n",
    "    q=setFromUnitVectors(vector,xaxis)\n",
    "    \n",
    "    v=applyQuaternion1( pV ,q )\n",
    "    \n",
    "    return Vector3(v.x,v.y,v.z)\n",
    "end\n",
    "\n",
    "function toAxisOriginalVector3(pV::Vector3,axis::Vector3)\n",
    "    \n",
    "    xaxis=Vector3(1.0,0.0,0.0)\n",
    "\n",
    "    vector=normalizeVector3(axis)\n",
    "\n",
    "    q=setFromUnitVectors(xaxis,vector)\n",
    "\n",
    "    v=applyQuaternion1( pV ,q )\n",
    "    \n",
    "    return Vector3(v.x,v.y,v.z)\n",
    "end\n",
    "\n",
    "function  toAxisXQuat(pQ::Quaternion,axis::Vector3)\n",
    "    xaxis=Vector3(1.0,0.0,0.0)\n",
    "\n",
    "    vector=normalizeVector3(axis)\n",
    "\n",
    "    q=setFromUnitVectors(vector,xaxis)\n",
    "        \n",
    "    pV=Vector3(pQ.x,pQ.y,pQ.z)\n",
    "    \n",
    "    v=applyQuaternion1( pV ,q )\n",
    "\n",
    "    return Quaternion(v.x,v.y,v.z,1.0)\n",
    "    \n",
    "end\n",