Metavoxel GPU Validation 3.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "using LinearAlgebra\n",
    "using Plots\n",
    "import JSON\n",
    "# using Quaternions\n",
    "using StaticArrays, Rotations\n",
    "using Distributed\n",
    "using StaticArrays, BenchmarkTools\n",
    "using Base.Threads\n",
    "using CUDAnative\n",
    "using CuArrays,CUDAdrv \n",
    "using Test\n",
    "import Base: +, * , -, ^"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Todo\n",
    "- create struct for material and get for each edge its properties\n",
    "- implement getTimestep (done)\n",
    "- implement on single voxel (done)\n",
    "- get reat E and L (done)\n",
    "- compare to Frame3dd"
   ]
  },
  {
   "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": [
    {
     "data": {
      "text/plain": [
       "simulateParallel (generic function with 2 methods)"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function simulateParallel(numTimeSteps,dt)\n",
    "    # initialize(setup)\n",
    "    \n",
    "    for i in 1:numTimeSteps\n",
    "        #println(\"Timestep:\",i)\n",
    "        doTimeStep(dt,i)\n",
    "    end\n",
    "end\n",
    "\n",
    "function simulateParallel(metavoxel,numTimeSteps,dt)\n",
    "    # initialize(setup)\n",
    "    \n",
    "    for i in 1:numTimeSteps\n",
    "        #println(\"Timestep:\",i)\n",
    "        doTimeStep(metavoxel,dt,i)\n",
    "    end\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "initialize (generic function with 1 method)"
      ]
     },
     "execution_count": 4,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function initialize(setup)\n",
    "    nodes      = setup[\"nodes\"]\n",
    "    edges      = setup[\"edges\"]\n",
    "\n",
    "    i=1\n",
    "    # pre-calculate current position\n",
    "    for node in nodes\n",
    "        # element=parse(Int,node[\"id\"][2:end])\n",
    "        N_position[i]=Vector3(node[\"position\"][\"x\"]/100.0,node[\"position\"][\"y\"]/100.0,node[\"position\"][\"z\"]/100.0)\n",
    "        N_restrained[i]=node[\"restrained_degrees_of_freedom\"][1] ## todo later consider other degrees of freedom\n",
    "        N_displacement[i]=Vector3(node[\"displacement\"][\"x\"],node[\"displacement\"][\"y\"],node[\"displacement\"][\"z\"])\n",
    "        N_angle[i]=Vector3(node[\"angle\"][\"x\"],node[\"angle\"][\"y\"],node[\"angle\"][\"z\"])\n",
    "        N_force[i]=Vector3(node[\"force\"][\"x\"]/1,node[\"force\"][\"y\"]/1,node[\"force\"][\"z\"]/1)\n",
    "        N_currPosition[i]=Vector3(node[\"position\"][\"x\"]/100.0,node[\"position\"][\"y\"]/100.0,node[\"position\"][\"z\"]/100.0)\n",
    "\n",
    "        # for dynamic simulations\n",
    "        # append!(N_posTimeSteps,[[]])\n",
    "        # append!(N_angTimeSteps,[[]])\n",
    "\n",
    "        i=i+1\n",
    "    end \n",
    "\n",
    "    i=1\n",
    "    # pre-calculate the axis\n",
    "    for edge in edges\n",
    "        # element=parse(Int,edge[\"id\"][2:end])\n",
    "\n",
    "        # find the nodes that the lements connects\n",
    "        fromNode = nodes[edge[\"source\"]+1]\n",
    "        toNode = nodes[edge[\"target\"]+1]\n",
    "\n",
    "\n",
    "        node1 = [fromNode[\"position\"][\"x\"]/100.0 fromNode[\"position\"][\"y\"]/100.0 fromNode[\"position\"][\"z\"]/100.0]\n",
    "        node2 = [toNode[\"position\"][\"x\"]/100.0 toNode[\"position\"][\"y\"]/100.0 toNode[\"position\"][\"z\"]/100.0]\n",
    "\n",
    "        length=norm(node2-node1)\n",
    "        axis=normalize(collect(Iterators.flatten(node2-node1)))\n",
    "\n",
    "        E_source[i]=edge[\"source\"]+1\n",
    "        E_target[i]=edge[\"target\"]+1\n",
    "        E_area[i]=edge[\"area\"]\n",
    "        E_density[i]=edge[\"density\"]\n",
    "        E_stiffness[i]=edge[\"stiffness\"]\n",
    "        E_axis[i]=Vector3(axis[1],axis[2],axis[3])\n",
    "        E_currentRestLength[i]=length #?????????? todo change\n",
    "#         E_currentRestLength[i]=75/sqrt(2)\n",
    "\n",
    "        N_edgeID[E_source[i],N_currEdge[E_source[i]]]=i\n",
    "        N_edgeFirst[E_source[i],N_currEdge[E_source[i]]]=true\n",
    "        N_currEdge[E_source[i]]+=1\n",
    "\n",
    "        N_edgeID[E_target[i],N_currEdge[E_target[i]]]=i\n",
    "        N_edgeFirst[E_target[i],N_currEdge[E_target[i]]]=false\n",
    "        N_currEdge[E_target[i]]+=1\n",
    "\n",
    "\n",
    "        # for dynamic simulations\n",
    "        # append!(E_stressTimeSteps,[[]])\n",
    "\n",
    "        i=i+1\n",
    "    end \n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "doTimeStep! (generic function with 1 method)"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function doTimeStep(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!(\n",
    "        E_sourceGPU, \n",
    "        E_targetGPU,\n",
    "        E_areaGPU,\n",
    "        E_densityGPU,\n",
    "        E_stiffnessGPU,\n",
    "        E_stressGPU,\n",
    "        E_axisGPU,\n",
    "        E_currentRestLengthGPU,\n",
    "        E_pos2GPU,\n",
    "        E_angle1vGPU,\n",
    "        E_angle2vGPU,\n",
    "        E_angle1GPU,\n",
    "        E_angle2GPU,\n",
    "        E_intForce1GPU,\n",
    "        E_intMoment1GPU,\n",
    "        E_intForce2GPU,\n",
    "        E_intMoment2GPU,\n",
    "        E_dampGPU,\n",
    "        N_currPositionGPU,\n",
    "        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",
    "        N_positionGPU, \n",
    "        N_restrainedGPU,\n",
    "        N_displacementGPU,\n",
    "        N_angleGPU,\n",
    "        N_currPositionGPU,\n",
    "        N_linMomGPU,\n",
    "        N_angMomGPU,\n",
    "        N_intForceGPU,\n",
    "        N_intMomentGPU,\n",
    "        N_forceGPU,\n",
    "        N_momentGPU,\n",
    "        N_orientGPU,\n",
    "        N_edgeIDGPU, \n",
    "        N_edgeFirstGPU, \n",
    "        E_intForce1GPU,\n",
    "        E_intMoment1GPU,\n",
    "        E_intForce2GPU,\n",
    "        E_intMoment2GPU)\n",
    "    \n",
    "end\n",
    "\n",
    "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!(\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[\"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[\"E_intForce1GPU\"],\n",
    "        metavoxel[\"E_intMoment1GPU\"],\n",
    "        metavoxel[\"E_intForce2GPU\"],\n",
    "        metavoxel[\"E_intMoment2GPU\"])\n",
    "    \n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "run_updateEdges! (generic function with 1 method)"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function updateEdges!(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,\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",
    "        nu=0\n",
    "#         L = 5.0 #?? change!!!!!!\n",
    "        L=l\n",
    "        a1 = E*L # EA/L : Units of N/m\n",
    "        a2 = E * L*L*L / (12.0*(1+nu)) # GJ/L : Units of N-m\n",
    "        b1 = E*L # 12EI/L^3 : Units of N/m\n",
    "        b2 = E*L*L/2.0 # 6EI/L^2 : Units of N (or N-m/m: torque related to linear distance)\n",
    "        b3 = E*L*L*L/6.0 # 2EI/L : Units of N-m\n",
    "        \n",
    "        nu=0.35\n",
    "        W = 75\n",
    "#         L = W/sqrt(2)\n",
    "        l=L\n",
    "        n_min = 1\n",
    "        n_max = 7\n",
    "        # Cross Section inputs, must be floats\n",
    "        mass=125000 #before for voxel\n",
    "        mass=1\n",
    "        E = 2000e9  # MPa\n",
    "        G = E * 1 / 3  # MPa\n",
    "        h = 2.38/100.0  # mm\n",
    "        b = 2.38/100.0 # mm\n",
    "        rho = 7.85e-9 / 3  # kg/mm^3\n",
    "        S = h * b\n",
    "        Sy = (S * (6 + 12 * nu + 6 * nu^2)/ (7 + 12 * nu + 4 * nu^2))\n",
    "        # For solid rectangular cross section (width=b, depth=d & ( b < d )):\n",
    "        Q = 1 / 3 - 0.2244 / (min(h / b, b / h) + 0.1607)\n",
    "        Jxx = Q * min(h * b^3, b * h^3)\n",
    "        s=b\n",
    "    \n",
    "        MaxFreq2=E*s/mass\n",
    "        dt= 1/(6.283185*sqrt(MaxFreq2))\n",
    "\n",
    "\n",
    "        ##if voxels\n",
    "        #nu=0\n",
    "        #L=l\n",
    "        #a1 = E*L # EA/L : Units of N/m\n",
    "        #a2 = E * L*L*L / (12.0*(1+nu)) # GJ/L : Units of N-m\n",
    "        #b1 = E*L # 12EI/L^3 : Units of N/m\n",
    "        #b2 = E*L*L/2.0 # 6EI/L^2 : Units of N (or N-m/m: torque related to linear distance)\n",
    "        #b3 = E*L*L*L/6.0 # 2EI/L : Units of N-m\n",
    "\n",
    "        I= b*h^3/12\n",
    "        J=b*h*(b*b+h*h)/12\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",
    "        \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",
    "            sqA1=CUDAnative.sqrt(a1) \n",
    "            sqA2xIp=CUDAnative.sqrt(a2*L*L/6.0) \n",
    "            sqB1=CUDAnative.sqrt(b1) \n",
    "            sqB2xFMp=CUDAnative.sqrt(b2*L/2) \n",
    "            sqB3xIp=CUDAnative.sqrt(b3*L*L/6.0)\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= 2*sqrt(mass)*zeta/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!(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,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!(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,N_currPosition,N_orient)\n",
    "    end\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "run_updateNodes! (generic function with 1 method)"
      ]
     },
     "execution_count": 7,
     "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_intForce,N_intMoment,N_force,N_moment,N_orient,N_edgeID,N_edgeFirst,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",
    "        @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",
    "            @inbounds curForce = force(N_intForce[i],N_orient[i],N_force[i],true,currentTimeStep)\n",
    "            \n",
    "            #@inbounds N_force[i]=Vector3(0,0,0) ##????\n",
    "            \n",
    "            @inbounds N_intForce[i]=Vector3(0,0,0)\n",
    "        \n",
    "            #x=curForce.x\n",
    "            #y=curForce.y\n",
    "            #z=curForce.z\n",
    "            #@cuprintln(\"curForce x $x, y $y, z $z \")\n",
    "            \n",
    "            #x=N_linMom[i].x*10000000000\n",
    "            #y=N_linMom[i].y*10000000000\n",
    "            #z=N_linMom[i].z*10000000000\n",
    "            #@cuprintln(\"N_linMom[i] x $x, y $y, z $z \")\n",
    "            \n",
    "            \n",
    "            @inbounds N_linMom[i]=N_linMom[i]+curForce*Vector3(dt,dt,dt) #todo make sure right\n",
    "            massInverse=8e-6 # todo ?? later change //8e-6\n",
    "            mass=1\n",
    "            massInverse=1.0/mass #\n",
    "            @inbounds translate=N_linMom[i]*Vector3((dt*massInverse),(dt*massInverse),(dt*massInverse)) # ??massInverse\n",
    "            \n",
    "            #x=translate.x*10000000000\n",
    "            #y=translate.y*10000000000\n",
    "            #z=translate.z*10000000000\n",
    "            #@cuprintln(\"translate x $x, y $y, z $z \")\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",
    "            \n",
    "            \n",
    "            \n",
    "            momentInertiaInverse=1.92e-6 # todo ?? later change 1/Inertia (1/(kg*m^2))\n",
    "            \n",
    "            \n",
    "            @inbounds temp=FromRotationVector(N_angMom[i]*Vector3((dt*momentInertiaInverse),(dt*momentInertiaInverse),(dt*momentInertiaInverse)))\n",
    "            \n",
    "            \n",
    "            #x=temp.x*10000000000\n",
    "            #y=temp.y*10000000000\n",
    "            #z=temp.z*10000000000\n",
    "            #@cuprintln(\"temp x $x, y $y, z $z \")\n",
    "            \n",
    "            @inbounds N_orient[i]=multiplyQuaternions(temp,N_orient[i])\n",
    "            \n",
    "            #@inbounds x= N_orient[i].x*temp.x\n",
    "            #@inbounds y= N_orient[i].y*temp.y\n",
    "            #@inbounds z= N_orient[i].z*temp.z\n",
    "            #@inbounds w= N_orient[i].w*temp.w\n",
    "            #x=convert(Float64,x)\n",
    "            #y=convert(Float64,y)\n",
    "            #z=convert(Float64,z)\n",
    "            #w=convert(Float64,w)\n",
    "            \n",
    "            #@inbounds N_orient[i]=Quaternion(x,y,z,w)\n",
    "            \n",
    "            #x=N_orient[i].x*10000000000\n",
    "            #y=N_orient[i].y*10000000000\n",
    "            #z=N_orient[i].z*10000000000\n",
    "            #w=N_orient[i].w\n",
    "            #@cuprintln(\"N_orient x $x, y $y, z $z, w $w \")\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_linMom,N_angMom,N_intForce,N_intMoment,N_force,N_moment,N_orient,N_edgeID,N_edgeFirst,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_angle,N_currPosition,N_linMom,N_angMom,N_intForce,N_intMoment,N_force,N_moment,N_orient,N_edgeID,N_edgeFirst,E_intForce1,E_intMoment1,E_intForce2,E_intMoment2)\n",
    "    end\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "orientLink! (generic function with 1 method)"
      ]
     },
     "execution_count": 8,
     "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",
    "    #x=angle1.x*10000000000\n",
    "    #y=angle1.y*10000000000\n",
    "    #z=angle1.z*10000000000\n",
    "    #@cuprintln(\"angle1 x $x, y $y, z $z \")\n",
    "    #x=oVNeg.x*10000000000\n",
    "    #y=oVNeg.y*10000000000\n",
    "    #z=oVNeg.z*10000000000\n",
    "    #@cuprintln(\"oVNeg x $x, y $y, z $z \")\n",
    "    \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",
    "    #x=pos2.x*10000000000\n",
    "    #y=pos2.y*10000000000\n",
    "    #z=pos2.z*10000000000\n",
    "    #@cuprintln(\"pos2 2 x $x, y $y, z $z \")\n",
    "    \n",
    "    \n",
    "    #x=totalRot.x*10000000000\n",
    "    #y=totalRot.y*10000000000\n",
    "    #z=totalRot.z*10000000000\n",
    "    #@cuprintln(\"totalRot x $x, y $y, z $z \")\n",
    "    \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",
    "    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",
    "    \n",
    "    \n",
    "#     pos2,angle1v,angle2v,angle1,angle2,\n",
    "    return pos2,angle1v,angle2v,angle1,angle2,totalRot\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "toAxisOriginalQuat (generic function with 1 method)"
      ]
     },
     "execution_count": 9,
     "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",
    "\n",
    "    vector=normalizeVector3(axis)\n",
    "    q=setFromUnitVectors(vector,xaxis)\n",
    "    \n",
    "    \n",
    " \n",
    "#     rot=setFromRotationMatrix(quatToMatrix( q  ))\n",
    "    \n",
    "    \n",
    "#     v= applyQuaternion1( pV ,setQuaternionFromEuler(rot) )\n",
    "    v=applyQuaternion1( pV ,q )\n",
    "    \n",
    "    #d=15\n",
    "    #vx=round(v.x, digits=d)\n",
    "    #vy=round(v.y, digits=d)\n",
    "    #vz=round(v.z, digits=d)\n",
    "    \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",
    "\n",
    "#     rot=setFromRotationMatrix(quatToMatrix( q  ))\n",
    "\n",
    "#     v= applyQuaternion1( pV ,setQuaternionFromEuler(rot) )\n",
    "    v=applyQuaternion1( pV ,q )\n",
    "    \n",
    "    #d=15\n",
    "    #vx=round(v.x, digits=d)\n",
    "    #vy=round(v.y, digits=d)\n",
    "    #vz=round(v.z, digits=d)\n",
    "    \n",
    "    \n",
    "    return Vector3(v.x,v.y,v.z)\n",
    "end\n",
    "\n",
    "function  toAxisXQuat(pQ::Quaternion,axis::Vector3)\n",
    "    \n",
    "    xaxis=Vector3(1.0,0.0,0.0)\n",
    "\n",
    "    vector=normalizeVector3(axis)\n",
    "\n",
    "\n",
    "    q=setFromUnitVectors(vector,xaxis)\n",
    "        \n",
    "    #d=17\n",
    "    #qw=round(q[1], digits=d)\n",
    "    #qx=round(q[2], digits=d)\n",
    "    #qy=round(q[3], digits=d)\n",
    "    #qz=round(q[4], digits=d)\n",
    "    #\n",
    "\n",
    "#     rot=setFromRotationMatrix(quatToMatrix( q  ))\n",
    "    \n",
    "    pV=Vector3(pQ.x,pQ.y,pQ.z)\n",
    "    \n",
    "#     v=applyQuaternion1( pV ,setQuaternionFromEuler(rot) )\n",
    "    v=applyQuaternion1( pV ,q )\n",
    "    \n",
    "    #d=15\n",
    "    #vx=round(v.x, digits=d)\n",
    "    #vy=round(v.y, digits=d)\n",
    "    #vz=round(v.z, digits=d)\n",
    "    \n",
    "    return Quaternion(v.x,v.y,v.z,1.0)\n",
    "    \n",
    "    # return [1.0 v[1] v[2] v[3]]\n",
    "end\n",
    "\n",
    "function toAxisOriginalQuat(pQ::Vector3,axis::Vector3)\n",
    "    xaxis=Vector3(1.0,0.0,0.0)\n",
    "\n",
    "    vector=normalizeVector3(axis)\n",
    "    \n",
    "    q=setFromUnitVectors(xaxis,vector)\n",
    "    \n",
    "\n",
    "#     rot=setFromRotationMatrix(quatToMatrix( q  ))\n",
    "    \n",
    "    pV=Vector3(pQ.x,pQ.y,pQ.z)\n",
    "#     v=applyQuaternion1( pV ,setQuaternionFromEuler(rot) )\n",
    "    v=applyQuaternion1( pV ,q )\n",
    "    \n",
    "    #d=15\n",
    "    #vx=round(v.x, digits=d)\n",
    "    #vy=round(v.y, digits=d)\n",
    "    #vz=round(v.z, digits=d)\n",
    "    \n",
    "    return Quaternion(v.x,v.y,v.z,1.0)\n",
    "    \n",
    "end"
   ]