Parallel FEA 2.ipynb

{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": 148,
   "metadata": {},
   "outputs": [],
   "source": [
    "using LinearAlgebra\n",
    "using Plots\n",
    "import JSON\n",
    "using StaticArrays, Rotations\n",
    "# BASED ON https://github.com/jonhiller/Voxelyze"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 149,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "simulateParallel (generic function with 3 methods)"
      ]
     },
     "execution_count": 149,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function simulateParallel(setup,numTimeSteps,dt,static=true,saveInterval=10)\n",
    "\tinitialize(setup)\n",
    "    for i in 1:numTimeSteps\n",
    "        # println(\"Timestep:\",i)\n",
    "        doTimeStep(setup,dt,static,i,saveInterval);\n",
    "    end\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 150,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "initialize (generic function with 1 method)"
      ]
     },
     "execution_count": 150,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function initialize(setup)\n",
    "\tnodes      = setup[\"nodes\"]\n",
    "    edges      = setup[\"edges\"]\n",
    "\t# pre-calculate current position\n",
    "\tfor node in nodes\n",
    "        # element=parse(Int,node[\"id\"][2:end])\n",
    "\n",
    "        append!(N_position,[[node[\"position\"][\"x\"] node[\"position\"][\"y\"] node[\"position\"][\"z\"]]])\n",
    "        append!(N_degrees_of_freedom,[node[\"degrees_of_freedom\"]])\n",
    "        append!(N_restrained_degrees_of_freedom, [node[\"restrained_degrees_of_freedom\"]])\n",
    "        append!(N_displacement,[[node[\"displacement\"][\"x\"] node[\"displacement\"][\"y\"] node[\"displacement\"][\"z\"]]])\n",
    "        append!(N_angle,[[node[\"angle\"][\"x\"] node[\"angle\"][\"y\"] node[\"angle\"][\"z\"]]])\n",
    "        append!(N_force,[[node[\"force\"][\"x\"] node[\"force\"][\"y\"] node[\"force\"][\"z\"]]])\n",
    "        append!(N_currPosition,[[node[\"position\"][\"x\"] node[\"position\"][\"y\"] node[\"position\"][\"z\"]]])\n",
    "        append!(N_orient,[[1.0 0.0 0.0 0.0]])#quat\n",
    "        append!(N_linMom,[[0 0 0]])\n",
    "        append!(N_angMom,[[0 0 0]])\n",
    "        append!(N_intForce,[[0 0 0]])\n",
    "        append!(N_intMoment,[[0 0 0]])\n",
    "        append!(N_moment,[[0 0 0]])\n",
    "        \n",
    "        # for dynamic simulations\n",
    "        append!(N_posTimeSteps,[[]])\n",
    "        append!(N_angTimeSteps,[[]])\n",
    "        \n",
    "\tend \n",
    "\t# pre-calculate the axis\n",
    "\tfor 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\"] fromNode[\"position\"][\"y\"] fromNode[\"position\"][\"z\"]]\n",
    "        node2 = [toNode[\"position\"][\"x\"] toNode[\"position\"][\"y\"] toNode[\"position\"][\"z\"]]\n",
    "        \n",
    "        length=norm(node2-node1)\n",
    "        axis=normalize(collect(Iterators.flatten(node2-node1)))\n",
    "        \n",
    "        append!(E_source,[edge[\"source\"]+1])\n",
    "        append!(E_target,[edge[\"target\"]+1])\n",
    "        append!(E_area,[edge[\"area\"]])\n",
    "#         append!(E_density,[edge[\"density\"]])\n",
    "#         append!(E_stiffness,[edge[\"stiffness\"]])\n",
    "        append!(E_density,[1000])\n",
    "        append!(E_stiffness,[1000000])\n",
    "        append!(E_stress,[0])\n",
    "        append!(E_axis,[axis])\n",
    "        append!(E_currentRestLength,[length])\n",
    "        append!(E_pos2,[[0 0 0]])\n",
    "        append!(E_angle1v,[[0 0 0]])\n",
    "        append!(E_angle2v,[[0 0 0]])\n",
    "        append!(E_angle1,[[1.0 0.0 0.0 0.0]]) #quat\n",
    "        append!(E_angle2,[[1.0 0.0 0.0 0.0]]) #quat\n",
    "        append!(E_currentTransverseStrainSum,[0])\n",
    "        \n",
    "        # for dynamic simulations\n",
    "        append!(E_stressTimeSteps,[[]])\n",
    "        \n",
    "\tend \n",
    "\t\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 151,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "doTimeStep (generic function with 1 method)"
      ]
     },
     "execution_count": 151,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function doTimeStep(setup,dt,static,currentTimeStep,saveInterval)\n",
    "    \n",
    "    nodes      = setup[\"nodes\"]\n",
    "    edges      = setup[\"edges\"]\n",
    "    voxCount=size(nodes)[1]\n",
    "    linkCount=size(edges)[1]\n",
    "\n",
    "\tif dt==0 \n",
    "\t\treturn true\n",
    "\telseif (dt<0) \n",
    "\t\tdt = recommendedTimeStep()\n",
    "    end\n",
    "\n",
    "\t# if (collisions) updateCollisions();\n",
    "\tcollisions=false\n",
    "\n",
    "\t# Euler integration:\n",
    "\tDiverged = false\n",
    "    #  for edge in edges\n",
    "    \n",
    "\tfor i in 1:linkCount\n",
    "        # fromNode = nodes[edge[\"source\"]+1]\n",
    "        # toNode = nodes[edge[\"target\"]+1]\n",
    "        # node1 = [fromNode[\"position\"][\"x\"] fromNode[\"position\"][\"y\"] fromNode[\"position\"][\"z\"]]\n",
    "        # node2 = [toNode[\"position\"][\"x\"] toNode[\"position\"][\"y\"] toNode[\"position\"][\"z\"]]\n",
    "        # updateForces(setup,edge,node1,node2,static)# element numbers??\n",
    "        updateForces(setup,i,static)# element numbers??\n",
    "        #  todo: update forces and whatever\n",
    "\t\tif axialStrain(true) > 100\n",
    "\t\t\tDiverged = true; # catch divergent condition! (if any thread sets true we will fail, so don't need mutex...\n",
    "        end\n",
    "    end\n",
    "    \n",
    "    \n",
    "    if Diverged\n",
    "\t\tprintln(\"Divergedd!!!!!\")\n",
    "\t\treturn false\n",
    "    end\n",
    "                \n",
    "\tfor i in 1:voxCount\n",
    "\t\ttimeStep(dt,i,static,currentTimeStep)\n",
    "        # timeStep(dt,node,static,currentTimeStep)\n",
    "\t\tif(!static&& currentTimeStep%saveInterval==0)\n",
    "            append!(N_posTimeSteps[i],[N_displacement[i]])\n",
    "            append!(N_angTimeSteps[i],[N_angle[i]])\n",
    "\n",
    "        end\n",
    "\t\t#  todo: update linMom,angMom, orient and whatever\n",
    "    end\n",
    "\n",
    "\t\n",
    "\tcurrentTimeStep = currentTimeStep+dt\n",
    "\treturn true\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 152,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "updateForces (generic function with 2 methods)"
      ]
     },
     "execution_count": 152,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function updateForces(setup,edge,static=true)\n",
    "    \n",
    "\t# pVNeg=new THREE.Vector3(node1.position.x,node1.position.y,node1.position.z);\n",
    "\t# pVPos=new THREE.Vector3(node2.position.x,node2.position.y,node2.position.z);\n",
    "    # currentRestLength=pVPos.clone().sub(pVNeg).length();\n",
    "\t# edge.currentRestLength=currentRestLength; # todo make sure updated\n",
    "    \n",
    "\tnode1=E_source[edge]\n",
    "    node2=E_target[edge]\n",
    "    \n",
    "    currentRestLength=E_currentRestLength[edge]\n",
    "    \n",
    "    \n",
    "\tpVNeg=copy(N_currPosition[node1])# todo change to be linked to edge not node \n",
    "\tpVPos=copy(N_currPosition[node2])# todo change to be linked to edge not node\n",
    "    \n",
    "    \n",
    "\t#  Vec3D<double> three\n",
    "\toldPos2 = copy(E_pos2[edge])\n",
    "\toldAngle1v = copy(E_angle1v[edge])\n",
    "\toldAngle2v =  copy(E_angle2v[edge])# remember the positions/angles from last timestep to calculate velocity\n",
    "\t#  var oldAngle1v=new THREE.Vector3(node1.angle.x,node1.angle.y,node1.angle.z);//?\n",
    "\t#  var oldAngle2v=new THREE.Vector3(node2.angle.x,node2.angle.y,node2.angle.z); //??\n",
    "    \n",
    "\ttotalRot= orientLink(edge) # sets pos2, angle1, angle2 /*restLength*/\n",
    "    \n",
    "\n",
    "\tdPos2=0.5*(copy(E_pos2[edge])-oldPos2)\n",
    "\tdAngle1=0.5*(copy(E_angle1v[edge])-oldAngle1v)\n",
    "\tdAngle2=0.5*(copy(E_angle2v[edge])-oldAngle2v)\n",
    "    \n",
    "    \n",
    "\n",
    "\t# if volume effects...\n",
    "    # if (!mat->isXyzIndependent() || currentTransverseStrainSum != 0) \n",
    "    # updateTransverseInfo(); //currentTransverseStrainSum != 0 catches when we disable poissons mid-simulation\n",
    "\n",
    "\t\n",
    "    _stress=updateStrain((E_pos2[edge][1]/E_currentRestLength[edge]),E_stiffness[edge])\n",
    "    #  var _stress=updateStrain(1.0);\n",
    "    \n",
    "\n",
    "\tE_stress[edge] = _stress\n",
    "\tif !static\n",
    "        append!(E_stressTimeSteps[edge],[_stress])\n",
    "    end\n",
    "    \n",
    "    ######### check this\n",
    "\tif setup[\"viz\"][\"minStress\"]>_stress\n",
    "\t\tsetup[\"viz\"][\"minStress\"]=_stress\n",
    "\telseif setup[\"viz\"][\"maxStress\"]<_stress\n",
    "\t\tsetup[\"viz\"][\"maxStress\"]=_stress\n",
    "    end\n",
    "    \n",
    "    \n",
    "\t#  if (isFailed()){forceNeg = forcePos = momentNeg = momentPos = Vec3D<double>(0,0,0); return;}\n",
    "\n",
    "\t#  var b1=mat->_b1, b2=mat->_b2, b3=mat->_b3, a2=mat->_a2; //local copies //todo get from where i had\n",
    "\t\n",
    "\tl   = currentRestLength # ??\n",
    "\trho = E_density[edge] \n",
    "\tA = E_area[edge] \n",
    "\tE = E_stiffness[edge] #  youngs modulus\n",
    "\tG=1.0 # todo shear_modulus\n",
    "\tixx = 1.0 # todo section ixx\n",
    "\tI=1.0\n",
    "\tiyy = 1.0  # todo section.iyy//\n",
    "\t#  var l0=length.dataSync();\n",
    "\tJ=1.0;# todo check\n",
    "\t#  var l02 = l0 * l0;\n",
    "\t#  var l03 = l0 * l0 * l0;\n",
    "\tb1= 12*E*I/(l*l*l)\n",
    "\tb2= 6*E*I/(l*l)\n",
    "\tb3= 2*E*I/(l)\n",
    "\ta1= E*A/l\n",
    "\ta2= G*J/l\n",
    "\tnu=0\n",
    "\n",
    "\tb1= 5e6\n",
    "\tb2= 1.25e7\n",
    "\tb3= 2.08333e+07\n",
    "\ta1= E*A/l\n",
    "\ta2= 1.04167e+07\n",
    "\n",
    "\tL = 5;\n",
    "\ta1 = E*L # EA/L : Units of N/m\n",
    "\ta2 = E * L*L*L / (12.0*(1+nu)) # GJ/L : Units of N-m\n",
    "\tb1 = E*L # 12EI/L^3 : Units of N/m\n",
    "\tb2 = E*L*L/2.0 # 6EI/L^2 : Units of N (or N-m/m: torque related to linear distance)\n",
    "\tb3 = E*L*L*L/6.0 # 2EI/L : Units of N-m\n",
    "\t# console.log(\"currentRestLength:\"+currentRestLength);\n",
    "\t# console.log(\"b1:\"+b1/10e6);\n",
    "\t# console.log(\"b2:\"+b2/10e7);\n",
    "\t# console.log(\"b3:\"+b3/10e7);\n",
    "\t# console.log(\"a2:\"+a2/10e7);\n",
    "\t# var b1= 5e6;\n",
    "\t# var b2= 1.25e7;\n",
    "\t# var b3= 2.08333e+07;\n",
    "\t# var a1= E*A/l;\n",
    "\t# var a2= 1.04167e+07;\n",
    "    \n",
    "\tcurrentTransverseArea=25.0 #  todo ?? later change\n",
    "\tcurrentTransverseArea=A\n",
    "\n",
    "\t# Beam equations. All relevant terms are here, even though some are zero for small angle and others are zero for large angle (profiled as negligible performance penalty)\n",
    "    forceNeg = [(_stress*currentTransverseArea) (b1*E_pos2[edge][2]-b2*(E_angle1v[edge][3] + E_angle2v[edge][3])) (b1*E_pos2[edge][3] + b2*(E_angle1v[edge][2] + E_angle2v[edge][2]))] # Use Curstress instead of -a1*Pos2.x to account for non-linear deformation \n",
    "\tforcePos = -forceNeg;\n",
    "\n",
    "\tmomentNeg = [(a2*(E_angle2v[edge][1]-E_angle1v[edge][1])) (-b2*E_pos2[edge][3]-b3*(2*E_angle1v[edge][2]+E_angle2v[edge][2]))   (b2*E_pos2[edge][2] - b3*(2*E_angle1v[edge][3] + E_angle2v[edge][3]))]\n",
    "\tmomentPos = [(a2*(E_angle1v[edge][1]-E_angle2v[edge][1])) (-b2*E_pos2[edge][3]- b3*(E_angle1v[edge][2]+2*E_angle2v[edge][2]))  (b2*E_pos2[edge][2] - b3*(E_angle1v[edge][3] + 2*E_angle2v[edge][3]))]\n",
    "\t\n",
    "\t\t\t\t\t\t\t\t\n",
    "\t# //local damping:\n",
    "\t# if (isLocalVelocityValid()){ //if we don't have the basis for a good damping calculation, don't do any damping.\n",
    "\t# \tfloat sqA1=mat->_sqA1, sqA2xIp=mat->_sqA2xIp,sqB1=mat->_sqB1, sqB2xFMp=mat->_sqB2xFMp, sqB3xIp=mat->_sqB3xIp;\n",
    "\t# \tVec3D<double> posCalc(\tsqA1*dPos2.x,\n",
    "\t# \t\t\t\t\t\t\tsqB1*dPos2.y - sqB2xFMp*(dAngle1.z+dAngle2.z),\n",
    "\t# \t\t\t\t\t\t\tsqB1*dPos2.z + sqB2xFMp*(dAngle1.y+dAngle2.y));\n",
    "\n",
    "\t# \tforceNeg += pVNeg->dampingMultiplier()*posCalc;\n",
    "\t# \tforcePos -= pVPos->dampingMultiplier()*posCalc;\n",
    "\n",
    "\t# \tmomentNeg -= 0.5*pVNeg->dampingMultiplier()*Vec3D<>(\t-sqA2xIp*(dAngle2.x - dAngle1.x),\n",
    "\t# \t\t\t\t\t\t\t\t\t\t\t\t\t\t\tsqB2xFMp*dPos2.z + sqB3xIp*(2*dAngle1.y + dAngle2.y),\n",
    "\t# \t\t\t\t\t\t\t\t\t\t\t\t\t\t\t-sqB2xFMp*dPos2.y + sqB3xIp*(2*dAngle1.z + dAngle2.z));\n",
    "\t# \tmomentPos -= 0.5*pVPos->dampingMultiplier()*Vec3D<>(\tsqA2xIp*(dAngle2.x - dAngle1.x),\n",
    "\t# \t\t\t\t\t\t\t\t\t\t\t\t\t\t\tsqB2xFMp*dPos2.z + sqB3xIp*(dAngle1.y + 2*dAngle2.y),\n",
    "\t# \t\t\t\t\t\t\t\t\t\t\t\t\t\t\t-sqB2xFMp*dPos2.y + sqB3xIp*(dAngle1.z + 2*dAngle2.z));\n",
    "\n",
    "\t# }\n",
    "\t# else setBoolState(LOCAL_VELOCITY_VALID, true); //we're good for next go-around unless something changes\n",
    "\n",
    "    #\ttransform forces and moments to local voxel coordinates\n",
    "\tsmallAngle=false # ?? todo check\n",
    "    \n",
    "\n",
    "\tif !smallAngle # ?? chech\n",
    "\t\tforceNeg = RotateVec3DInv(E_angle1[edge],forceNeg)\n",
    "\t\tmomentNeg = RotateVec3DInv(E_angle1[edge],momentNeg)\n",
    "    end\n",
    "    \n",
    "\n",
    "    \n",
    "\n",
    "\n",
    "\tforcePos = RotateVec3DInv(E_angle2[edge],forcePos);\n",
    "\tmomentPos = RotateVec3DInv(E_angle2[edge],momentPos);\n",
    "\n",
    "    # println(momentPos)\n",
    "\n",
    "\tforceNeg =toAxisOriginalVector3(forceNeg,E_axis[edge]);\n",
    "\tforcePos =toAxisOriginalVector3(forcePos,E_axis[edge]);\n",
    "    \n",
    "\tmomentNeg=toAxisOriginalQuat(momentNeg,E_axis[edge]);# TODOO CHECKKKKKK\n",
    "\tmomentPos=toAxisOriginalQuat(momentPos,E_axis[edge]);\n",
    "    \n",
    "    # println(momentPos[2],\" \",momentPos[3],\" \",momentPos[4],\" \",momentPos[1],\" \")\n",
    "\n",
    "\tN_intForce[node1] =N_intForce[node1] +(forceNeg) ;\n",
    "\tN_intForce[node2] =N_intForce[node2] +(forcePos) ;\n",
    "    # println(N_intMoment[node2])\n",
    "\tN_intMoment[node1]=[(N_intMoment[node1][1]+momentNeg[2]) (N_intMoment[node1][2]+momentNeg[3]) (N_intMoment[node2][3]+momentPos[4])];\n",
    "\tN_intMoment[node2]=[(N_intMoment[node2][1]+momentPos[2]) (N_intMoment[node2][2]+momentPos[3]) (N_intMoment[node2][3]+momentPos[4])];\n",
    "    # println(N_intMoment[node2])\n",
    "\t# assert(!(forceNeg.x != forceNeg.x) || !(forceNeg.y != forceNeg.y) || !(forceNeg.z != forceNeg.z)); //assert non QNAN\n",
    "\t#  assert(!(forcePos.x != forcePos.x) || !(forcePos.y != forcePos.y) || !(forcePos.z != forcePos.z)); //assert non QNAN\n",
    "    \n",
    "\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 172,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "orientLink (generic function with 1 method)"
      ]
     },
     "execution_count": 172,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function orientLink( edge)  # updates pos2, angle1, angle2, and smallAngle //Quat3D<double> /*double restLength*/\n",
    "    node1=E_source[edge]\n",
    "    node2=E_target[edge]\n",
    "    currentRestLength=E_currentRestLength[edge]\n",
    "\tpVNeg=copy(N_currPosition[node1])# todo change to be linked to edge not node \n",
    "\tpVPos=copy(N_currPosition[node2])# todo change to be linked to edge not node\n",
    "\tpos2 = toAxisXVector3(pVPos-pVNeg,E_axis[edge]) # digit truncation happens here...\n",
    "    #  pos2.x = Math.round(pos2.x * 1e4) / 1e4; \n",
    "\tangle1 = toAxisXQuat(N_orient[node1],E_axis[edge])\n",
    "\tangle2 = toAxisXQuat(N_orient[node2],E_axis[edge])\n",
    "    # println(angle1[2],\" \",angle1[3],\" \",angle1[4],\" \",angle1[1])\n",
    "\ttotalRot = conjugate(angle1) #keep track of the total rotation of this bond (after toAxisX()) # Quat3D<double>\n",
    "    pos2 = RotateVec3D(totalRot,pos2)\n",
    "    print(\"pos2 \")\n",
    "    println(pos2)\n",
    "    \n",
    "\n",
    "\t# angle2 = copy(totalRot) .* angle2 # todo .*\n",
    "    angle2=[angle2[1]*totalRot[1] angle2[2]*totalRot[2] angle2[3]*totalRot[3] angle2[4]*totalRot[4]]\n",
    "\tangle1 = [1.0 0.0 0.0 0.0]#new THREE.Quaternion() #zero for now...\n",
    "    \n",
    "\n",
    "\t# small angle approximation?\n",
    "\t#  var SmallTurn =  ((Math.abs(pos2.z)+Math.abs(pos2.y))/pos2.x);\n",
    "\t#  var ExtendPerc = (Math.abs(1-pos2.x/currentRestLength));\n",
    "\t#  if (!smallAngle /*&& angle2.IsSmallAngle()*/ && SmallTurn < SA_BOND_BEND_RAD && ExtendPerc < SA_BOND_EXT_PERC){\n",
    "\t#  \tsmallAngle = true;\n",
    "\t#  \tsetBoolState(LOCAL_VELOCITY_VALID, false);\n",
    "\t#  }\n",
    "\t#  else if (smallAngle && (/*!angle2.IsSmallishAngle() || */SmallTurn > HYSTERESIS_FACTOR*SA_BOND_BEND_RAD || ExtendPerc > HYSTERESIS_FACTOR*SA_BOND_EXT_PERC)){\n",
    "\t#  \tsmallAngle = false;\n",
    "\t#  \tsetBoolState(LOCAL_VELOCITY_VALID, false);\n",
    "    #  }\n",
    "    \n",
    "    smallAngle=true #todo later remove\n",
    "\n",
    "\tif (smallAngle)\t #Align so Angle1 is all zeros\n",
    "\t\tpos2[1] =pos2[1]- currentRestLength #only valid for small angles\n",
    "    else  #Large angle. Align so that Pos2.y, Pos2.z are zero.\n",
    "\t\t# FromAngleToPosX(angle1,pos2) #get the angle to align Pos2 with the X axis\n",
    "\t\t# totalRot = angle1.clone().multiply(totalRot)  #update our total rotation to reflect this\n",
    "\t\t# angle2 = angle1.clone().multiply(  angle2) #rotate angle2\n",
    "\t\t# pos2 = new THREE.Vector3(pos2.length() - currentRestLength, 0, 0);\n",
    "    end\n",
    "    \n",
    "\n",
    "\tangle1v = ToRotationVector(angle1)\n",
    "\tangle2v = ToRotationVector(angle2)\n",
    "\n",
    "\t#  assert(!(angle1v.x != angle1v.x) || !(angle1v.y != angle1v.y) || !(angle1v.z != angle1v.z)); # assert non QNAN\n",
    "\t#  assert(!(angle2v.x != angle2v.x) || !(angle2v.y != angle2v.y) || !(angle2v.z != angle2v.z)); # assert non QNAN\n",
    "    \n",
    "    E_pos2[edge]=copy(pos2)\n",
    "    E_angle1v[edge]=copy(angle1v)\n",
    "    E_angle2v[edge]=copy(angle2v)\n",
    "    E_angle1[edge]=copy(angle1)\n",
    "    E_angle2[edge]=copy(angle2)\n",
    "    \n",
    "    print(\"angle1v \")\n",
    "    println(angle1v)\n",
    "    print(\"angle2v \")\n",
    "    println(angle2v)\n",
    "    \n",
    "\n",
    "\treturn totalRot\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 154,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "RotateVec3DInv (generic function with 1 method)"
      ]
     },
     "execution_count": 154,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function RotateVec3D(a, f)   \n",
    "\tfx= (f[1]==-0) ? 0 : f[1]\n",
    "    fy= (f[2]==-0) ? 0 : f[2]\n",
    "    fz= (f[3]==-0) ? 0 : f[3]\n",
    "    # fx= f[1]\n",
    "    # fy= f[2]\n",
    "    # fz= f[3]\n",
    "\ttw = fx*a[2] + fy*a[3] + fz*a[4]\n",
    "\ttx = fx*a[1] - fy*a[4] + fz*a[3]\n",
    "\tty = fx*a[4] + fy*a[1] - fz*a[2]\n",
    "\ttz = -fx*a[3] + fy*a[2] + fz*a[1]\n",
    "\n",
    "\treturn [(a[1]*tx+a[2][2]*tw+a[3]*tz-a[4]*ty) (a[1]*ty-a[2]*tz+a[3]*tw+a[4]*tx) (a[1]*tz+a[2]*ty-a[3]*tx + a[4]*tw)]\n",
    "end\n",
    "#!< Returns a vector representing the specified vector \"f\" rotated by this quaternion. @param[in] f The vector to transform.\n",
    "function RotateVec3DInv(a, f)  \n",
    "    fx=f[1]\n",
    "    fy=f[2]\n",
    "    fz=f[3]\n",
    "    tw = a[2]*fx + a[3]*fy + a[4]*fz\n",
    "    tx = a[1]*fx - a[3]*fz + a[4]*fy\n",
    "    ty = a[1]*fy + a[2]*fz - a[4]*fx\n",
    "    tz = a[1]*fz - a[2]*fy + a[3]*fx\n",
    "    return [(tw*a[2] + tx*a[1] + ty*a[4] - tz*a[3]) (tw*a[3] - tx*a[4] + ty*a[1] + tz*a[2]) (tw*a[4] + tx*a[3] - ty*a[2] + tz*a[1])]\t\n",
    "end\n",
    "#!< Returns a vector representing the specified vector \"f\" rotated by the inverse of this quaternion. This is the opposite of RotateVec3D. @param[in] f The vector to transform.\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 155,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "conjugate (generic function with 1 method)"
      ]
     },
     "execution_count": 155,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function setFromUnitVectors(vFrom, vTo )\n",
    "    # assumes direction vectors vFrom and vTo are normalized\n",
    "    EPS = 0.000001;\n",
    "    r =  dot(vFrom,vTo)+1\n",
    "\n",
    "    if r < EPS\n",
    "        r = 0;\n",
    "        if abs( vFrom[1] ) > abs( vFrom[3] ) \n",
    "            qx = - vFrom[2]\n",
    "            qy = vFrom[1]\n",
    "            qz = 0\n",
    "            qw = r\n",
    "        else \n",
    "            qx = 0\n",
    "            qy = - vFrom[3]\n",
    "            qz = vFrom[2]\n",
    "            qw = r\n",
    "        end\n",
    "   else \n",
    "        # crossVectors( vFrom, vTo ); // inlined to avoid cyclic dependency on Vector3\n",
    "        qx = vFrom[2] * vTo[3] - vFrom[3] * vTo[2]\n",
    "        qy = vFrom[3] * vTo[1] - vFrom[1] * vTo[3]\n",
    "        qz = vFrom[1] * vTo[2] - vFrom[2] * vTo[1]\n",
    "        qw = r\n",
    "\n",
    "    end\n",
    "    qx= (qx==-0) ? 0 : qx\n",
    "    qy= (qy==-0) ? 0 : qy\n",
    "    qz= (qz==-0) ? 0 : qz\n",
    "    qw= (qw==-0) ? 0 : qw\n",
    "    nn=normalize(collect(Iterators.flatten([qw,qx,qy,qz])))\n",
    "    return [nn[1] nn[2] nn[3] nn[4]]\n",
    "\n",
    "end\n",
    "\n",
    "function normalizeQ(q) \n",
    "    l = norm(q)\n",
    "    if l === 0 \n",
    "        qx = 0\n",
    "        qy = 0\n",
    "        qz = 0\n",
    "        qw = 1\n",
    "    else \n",
    "        l = 1 / l\n",
    "        qx = q[2] * l\n",
    "        qy = q[3] * l\n",
    "        qz = q[4] * l\n",
    "        qw = q[1] * l\n",
    "    end\n",
    "    return [qw qx qy qz]\n",
    "end\n",
    "\n",
    "function conjugate(q)\n",
    "    x= (-q[2]==-0) ? 0 : -q[2]\n",
    "    y= (-q[3]==-0) ? 0 : -q[3]\n",
    "    z= (-q[4]==-0) ? 0 : -q[4]\n",
    "\n",
    "    return [q[1] x y z]\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 156,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "applyQuaternion1 (generic function with 1 method)"
      ]
     },
     "execution_count": 156,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function applyQuaternion(q1,q2)\n",
    "    x = q1[2]\n",
    "    y = q1[3]\n",
    "    z = q1[4]\n",
    "    w = q1[1]\n",
    "    qx = q2[2]\n",
    "    qy = q2[3]\n",
    "    qz = q2[4]\n",
    "    qw = q2[1]\n",
    "\n",
    "    # calculate quat * vector\n",
    "\n",
    "    ix = qw * x + qy * z - qz * y\n",
    "    iy = qw * y + qz * x - qx * z\n",
    "    iz = qw * z + qx * y - qy * x\n",
    "    iw = - qx * x - qy * y - qz * z\n",
    "\n",
    "    # calculate result * inverse quat\n",
    "\n",
    "    xx = ix * qw + iw * - qx + iy * - qz - iz * - qy\n",
    "    yy = iy * qw + iw * - qy + iz * - qx - ix * - qz\n",
    "    zz = iz * qw + iw * - qz + ix * - qy - iy * - qx\n",
    "\n",
    "    mm=normalize(collect(Iterators.flatten([xx yy zz])))\n",
    "    return [mm[1] mm[2] mm[3]]\n",
    "end\n",
    "\n",
    "function applyQuaternion1(e,q2)\n",
    "    x = e[1]\n",
    "    y = e[2]\n",
    "    z = e[3]\n",
    "\n",
    "    qx = q2[2]\n",
    "    qy = q2[3]\n",
    "    qz = q2[4]\n",
    "    qw = q2[1]\n",
    "\n",
    "    # calculate quat * vector\n",
    "\n",
    "    ix = qw * x + qy * z - qz * y\n",
    "    iy = qw * y + qz * x - qx * z\n",
    "    iz = qw * z + qx * y - qy * x\n",
    "    iw = - qx * x - qy * y - qz * z\n",
    "\n",
    "    # calculate result * inverse quat\n",
    "\n",
    "    xx = ix * qw + iw * - qx + iy * - qz - iz * - qy\n",
    "    yy = iy * qw + iw * - qy + iz * - qx - ix * - qz\n",
    "    zz = iz * qw + iw * - qz + ix * - qy - iy * - qx\n",
    "\n",
    "    return [xx yy zz]\n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 157,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "setFromRotationMatrix (generic function with 1 method)"
      ]
     },
     "execution_count": 157,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function setQuaternionFromEuler(euler)\n",
    "    x=euler[1]\n",
    "    y=euler[2]\n",
    "    z=euler[3]\n",
    "    \n",
    "    c1 = cos( x / 2 )\n",
    "    c2 = cos( y / 2 )\n",
    "    c3 = cos( z / 2 )\n",
    "\n",
    "    s1 = sin( x / 2 )\n",
    "    s2 = sin( y / 2 )\n",
    "    s3 = sin( z / 2 )\n",
    "   \n",
    "    x = s1 * c2 * c3 + c1 * s2 * s3\n",
    "    y = c1 * s2 * c3 - s1 * c2 * s3\n",
    "    z = c1 * c2 * s3 + s1 * s2 * c3\n",
    "    w = c1 * c2 * c3 - s1 * s2 * s3\n",
    "    \n",
    "    return [w  x  y  z]\n",
    "end\n",
    "\n",
    "function quatToMatrix( quaternion )\n",
    "\n",
    "    te = zeros(16)\n",
    "\n",
    "    x = quaternion[2]\n",
    "    y = quaternion[3]\n",
    "    z = quaternion[4]\n",
    "    w = quaternion[1]\n",
    "    \n",
    "    x2 = x + x\n",
    "    y2 = y + y\n",
    "    z2 = z + z\n",
    "    xx = x * x2\n",
    "    xy = x * y2\n",
    "    xz = x * z2\n",
    "    yy = y * y2\n",
    "    yz = y * z2\n",
    "    zz = z * z2\n",
    "    wx = w * x2\n",
    "    wy = w * y2\n",
    "    wz = w * z2\n",
    "\n",
    "    sx = 1\n",
    "    sy = 1\n",
    "    sz = 1\n",
    "\n",
    "    te[ 1 ] = ( 1 - ( yy + zz ) ) * sx\n",
    "    te[ 2 ] = ( xy + wz ) * sx\n",
    "    te[ 3 ] = ( xz - wy ) * sx\n",
    "    te[ 4 ] = 0;\n",
    "\n",
    "    te[ 5 ] = ( xy - wz ) * sy\n",
    "    te[ 6 ] = ( 1 - ( xx + zz ) ) * sy\n",
    "    te[ 7 ] = ( yz + wx ) * sy\n",
    "    te[ 8 ] = 0;\n",
    "\n",
    "    te[ 9 ] = ( xz + wy ) * sz\n",
    "    te[ 10 ] = ( yz - wx ) * sz\n",
    "    te[ 11 ] = ( 1 - ( xx + yy ) ) * sz\n",
    "    te[ 12 ] = 0\n",
    "\n",
    "    te[ 13 ] = 0 #position.x;\n",
    "    te[ 14 ] = 0 #position.y;\n",
    "    te[ 15 ] = 0 #position.z;\n",
    "    te[ 16 ] = 1\n",
    "\n",
    "    return te\n",
    "\n",
    "end\n",
    "\n",
    "function  setFromRotationMatrix(m)\n",
    "    te = m\n",
    "    m11 = (te[ 1 ]== -0.0) ? 0.0 : te[ 1 ]\n",
    "    m12 = (te[ 5 ]== -0.0) ? 0.0 : te[ 5 ]\n",
    "    m13 = (te[ 9 ]== -0.0) ? 0.0 : te[ 9 ]\n",
    "    m21 = (te[ 2 ]== -0.0) ? 0.0 : te[ 2 ]\n",
    "    m22 = (te[ 6 ]== -0.0) ? 0.0 : te[ 6 ]\n",
    "    m23 = (te[ 10]== -0.0) ? 0.0 : te[ 10]\n",
    "    m31 = (te[ 3 ]== -0.0) ? 0.0 : te[ 3 ]\n",
    "    m32 = (te[ 7 ]== -0.0) ? 0.0 : te[ 7 ]\n",
    "    m33 = (te[ 11]== -0.0) ? 0.0 : te[ 11]\n",
    "\n",
    "    m11 = te[ 1 ]\n",
    "    m12 = te[ 5 ]\n",
    "    m13 = te[ 9 ]\n",
    "    m21 = te[ 2 ]\n",
    "    m22 = te[ 6 ]\n",
    "    m23 = te[ 10]\n",
    "    m31 = te[ 3 ]\n",
    "    m32 = te[ 7 ]\n",
    "    m33 = te[ 11]\n",
    "\n",
    "\n",
    "\n",
    "    y = asin( clamp( m13, - 1, 1 ) )\n",
    "\n",
    "    if ( abs( m13 ) < 0.9999999 ) \n",
    "        \n",
    "        x = atan( - m23, m33 )\n",
    "        z = atan( - m12, m11 )#-m12, m11\n",
    "        # if(m23==0.0)\n",
    "        #     x = atan( m23, m33 )\n",
    "        # end\n",
    "        # if(m12==0.0)\n",
    "        #     z = atan( m12, m11 )\n",
    "        # end\n",
    "\n",
    "    else\n",
    "\n",
    "        x = atan( m32, m22 )\n",
    "        z = 0;\n",
    "\n",
    "    end\n",
    "    \n",
    "    return [x y z]\n",
    "    \n",
    "end"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 158,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "toAxisXVector3 (generic function with 1 method)"
      ]
     },
     "execution_count": 158,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function toAxisOriginalVector3(pV,axis)\n",
    "    # xaxis=[1 0 0]\n",
    "    \n",
    "    # vector=copy(axis)\n",
    "    # vector=normalize(collect(Iterators.flatten(vector)))\n",
    "\n",
    "    # p = SVector(pV[1],pV[2], pV[3])\n",
    "    # q=setFromUnitVectors(xaxis, vector)\n",
    "    \n",
    "    # v= q * p\n",
    "    # return [v[1] v[2] v[3]]\n",
    "\n",
    "    xaxis=[1 0 0]\n",
    "\n",
    "    vector=copy(axis)\n",
    "    vector=normalize(collect(Iterators.flatten(vector)))\n",
    "\n",
    "    p = SVector(pV[1],pV[2], pV[3])    \n",
    "\n",
    "    q=setFromUnitVectors(xaxis, vector)\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",
    "    qw=q[1]\n",
    "    qx=q[2]\n",
    "    qy=q[3]\n",
    "    qz=q[4]\n",
    "\n",
    "    rot=setFromRotationMatrix(copy(quatToMatrix( copy([qw qx qy qz])  )))\n",
    "\n",
    "    return applyQuaternion1( copy(pV) ,setQuaternionFromEuler(copy(rot)) )\n",
    "end\n",
    "\n",
    "function toAxisXVector3(pV,axis) #TODO CHANGE\n",
    "    # xaxis=[1 0 0]\n",
    "    # vector=copy(axis)\n",
    "    # vector=normalize(collect(Iterators.flatten(vector)))\n",
    "    # p = SVector(pV[1],pV[2], pV[3])\n",
    "    # q=setFromUnitVectors(vector,xaxis)\n",
    "    \n",
    "    # v= q * p\n",
    "    # return [v[1] v[2] v[3]]\n",
    "\n",
    "    xaxis=[1 0 0]\n",
    "\n",
    "    vector=copy(axis)\n",
    "    vector=normalize(collect(Iterators.flatten(vector)))\n",
    "\n",
    "    p = SVector(pV[1],pV[2], pV[3])    \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",
    "    rot=setFromRotationMatrix(copy(quatToMatrix( copy([qw qx qy qz])  )))\n",
    "\n",
    "    return applyQuaternion1( copy(pV) ,setQuaternionFromEuler(copy(rot)) )\n",
    "end\n",
    "#transforms a vec3D in the original orientation of the bond to that as if the bond was in +X direction"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 159,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "toAxisXQuat (generic function with 1 method)"
      ]
     },
     "execution_count": 159,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function toAxisOriginalQuat(pQ,axis)\n",
    "    # xaxis=[1 0 0]  \n",
    "    # vector=copy(axis)\n",
    "    # vector=normalize(collect(Iterators.flatten(vector)))\n",
    "\n",
    "    # p = SVector(pQ[1],pQ[2], pQ[3])\n",
    "    # q=setFromUnitVectors(xaxis, vector)\n",
    "    \n",
    "    # v=q * p\n",
    "    # return Quat(1.0,v[1],v[2],v[3])\n",
    "\n",
    "    xaxis=[1 0 0]\n",
    "\n",
    "    vector=copy(axis)\n",
    "    vector=normalize(collect(Iterators.flatten(vector)))\n",
    "\n",
    "    \n",
    "    p = SVector(pQ[1],pQ[2], pQ[3])     \n",
    "\n",
    "    q=setFromUnitVectors(xaxis,vector)\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",
    "    qw=q[1]\n",
    "    qx=q[2]\n",
    "    qy=q[3]\n",
    "    qz=q[4]\n",
    "\n",
    "    rot=setFromRotationMatrix(copy(quatToMatrix( copy([qw qx qy qz])  )))\n",
    "    v=applyQuaternion1( copy([pQ[1] pQ[2] pQ[3]]) ,setQuaternionFromEuler(copy(rot)) )\n",
    "\n",
    "    return [1.0 v[1] v[2] v[3]]\n",
    "    \n",
    "end\n",
    "\n",
    "function  toAxisXQuat(pQ,axis)\n",
    "    # xaxis=[1 0 0]  \n",
    "    # vector=copy(axis)\n",
    "    # vector=normalize(collect(Iterators.flatten(vector)))\n",
    "\n",
    "    # p = SVector(q.x,q.y, q.z)\n",
    "    # q=setFromUnitVectors(vector,xaxis)\n",
    "    \n",
    "    # v=q * p\n",
    "    # return Quat(q.w,v[1],v[2],v[3])\n",
    "\n",
    "    xaxis=[1 0 0]\n",
    "\n",
    "    vector=copy(axis)\n",
    "    vector=normalize(collect(Iterators.flatten(vector)))\n",
    "\n",
    "    p = SVector(pQ[2],pQ[3],pQ[4])   \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",
    "    rot=setFromRotationMatrix(copy(quatToMatrix( copy([qw qx qy qz])  )))\n",
    "    v=applyQuaternion1( copy([pQ[2] pQ[3] pQ[4]]) ,setQuaternionFromEuler(copy(rot)) )\n",
    "    return [1.0 v[1] v[2] v[3]]\n",
    "    # return [1.0 v[1] v[2] v[3]]\n",
    "end\n",
    "#transforms a vec3D in the original orientation of the bond to that as if the bond was in +X direction\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 160,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "FromRotationVector (generic function with 1 method)"
      ]
     },
     "execution_count": 160,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "function ToRotationVector(a)  \n",
    "\tif (a[1] >= 1.0 || a[1] <= -1.0) \n",
    "\t\treturn [0 0 0]\n",
    "    end\n",
    "\tsquareLength = 1.0-a[1]*a[1]; # because x*x + y*y + z*z + w*w = 1.0, but more susceptible to w noise (when \n",
    "\tSLTHRESH_ACOS2SQRT= 2.4e-3; # SquareLength threshhold for when we can use square root optimization for acos. From SquareLength = 1-w*w. (calculate according to 1.0-W_THRESH_ACOS2SQRT*W_THRESH_ACOS2SQRT\n",
    "    \n",
    "\tif (squareLength < SLTHRESH_ACOS2SQRT) # ???????\n",