{
"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",
"\t\treturn [a[2] a[3] a[4]] *(2.0*sqrt((2-2*a[1])/squareLength)); # acos(w) = sqrt(2*(1-x)) for w close to 1. for w=0.001, error is 1.317e-6\n",
"\telse \n",
"\t\treturn [a[2] a[3] a[4]] * (2.0*acos(a[1])/sqrt(squareLength));\n",
" end \n",
"end \n",
"# !< Returns a rotation vector representing this quaternion rotation. Adapted from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/\n",
"\n",
"\n",
"function FromRotationVector(VecIn)\n",
" theta=VecIn/2.0\n",
" thetaMag2=norm(theta)*norm(theta)\n",
" DBL_EPSILONx24 =5.328e-15\n",
" if thetaMag2*thetaMag2 < DBL_EPSILONx24\n",
" qw=1.0 - 0.5*thetaMag2\n",
"\t\ts=1.0 - thetaMag2 / 6.0\n",
" else\n",
" thetaMag = sqrt(thetaMag2)\n",
"\t\tqw=cos(thetaMag)\n",
"\t\ts=sin(thetaMag) / thetaMag\n",
" end\n",
" qx=theta[1]*s\n",
" qy=theta[2]*s\n",
" qz=theta[3]*s;\n",
" \n",
" return [qw qx qy qz]\n",
"end\n"
]
},
{
"cell_type": "code",
"execution_count": 161,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"FromAngleToPosX (generic function with 1 method)"
]
},
"execution_count": 161,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"function FromAngleToPosX(a, RotateFrom) #highly optimized at the expense of readability\n",
" \n",
" SMALL_ANGLE_RAD= 1.732e-2 # Angles less than this get small angle approximations. To get: Root solve atan(t)/t-1+MAX_ERROR_PERCENT. From: MAX_ERROR_PERCENT = (t-atan(t))/t \n",
" SMALL_ANGLE_W =0.9999625 # quaternion W value corresponding to a SMALL_ANGLE_RAD. To calculate, cos(SMALL_ANGLE_RAD*0.5).\n",
" W_THRESH_ACOS2SQRT= 0.9988 # Threshhold of w above which we can approximate acos(w) with sqrt(2-2w). To get: Root solve 1-sqrt(2-2wt)/acos(wt) - MAX_ERROR_PERCENT. From MAX_ERROR_PERCENT = (acos(wt)-sqrt(2-2wt))/acos(wt)\n",
"\n",
" \n",
"\tif (RotateFrom[1]==0 && RotateFrom[2]==0 && RotateFrom[3]==0) \n",
"\t\treturn 0 #leave off if it slows down too much!!\n",
" end\n",
"\n",
" # Catch and handle small angle:\n",
" YoverX = RotateFrom[2]/RotateFrom[1]\n",
" ZoverX = RotateFrom[3]/RotateFrom[1]\n",
" if (YoverX<SMALL_ANGLE_RAD && YoverX>-SMALL_ANGLE_RAD && ZoverX<SMALL_ANGLE_RAD && ZoverX>-SMALL_ANGLE_RAD) # ??? //Intercept small angle and zero angle\n",
"\t\tax=0\n",
"\t\tay=0.5*ZoverX\n",
"\t\taz=-0.5*YoverX\n",
" aw = 1+0.5*(-a[3]*a[3]-a[4]*a[4]) # w=sqrt(1-x*x-y*y), small angle sqrt(1+x) ~= 1+x/2 at x near zero.\n",
" return [aw ax ay az]\n",
" end\n",
"\n",
" # more accurate non-small angle:\n",
" RotFromNorm = copy(RotateFrom)\n",
" RotFromNorm=normalize(collect(Iterators.flatten(RotFromNorm))) # Normalize the input...\n",
"\n",
" theta = acos(RotFromNorm[1]) # because RotFromNorm is normalized, 1,0,0 is normalized, and A.B = |A||B|cos(theta) = cos(theta)\n",
" if(theta >(π-DISCARD_ANGLE_RAD)) # ??????\n",
" aw=0\n",
"\t\tax=0\n",
"\t\tay=1\n",
"\t\taz=0\n",
" return [aw ax ay az]\n",
" end # 180 degree rotation (about y axis, since the vector must be pointing in -x direction\n",
"\n",
" AxisMagInv = 1.0/sqrt(RotFromNorm[3]*RotFromNorm[3]+RotFromNorm[2]*RotFromNorm[2])\n",
" # Here theta is the angle, axis is RotFromNorm.Cross(Vec3D(1,0,0)) = Vec3D(0, RotFromNorm.z/AxisMagInv, -RotFromNorm.y/AxisMagInv), which is still normalized. (super rolled together)\n",
" aa = 0.5*theta\n",
" s = sin(a)\n",
"\taw= cos(aa)\n",
"\tax= 0\n",
"\tay= RotFromNorm[3]*AxisMagInv*s\n",
"\taz = -RotFromNorm[2]*AxisMagInv*s # angle axis function, reduced\n",
"\treturn [aw ax ay az]\n",
"\n",
"end\n",
" # !< Overwrites this quaternion with the calculated rotation that would transform the specified RotateFrom vector to point in the positve X direction. Note: function changes this quaternion. @param[in] RotateFrom An arbitrary direction vector. Does not need to be normalized.\n"
]
},
{
"cell_type": "code",
"execution_count": 162,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"stress (generic function with 1 method)"
]
},
"execution_count": 162,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
" function axialStrain(positiveEnd)\n",
"\t# strainRatio = pVPos->material()->E/pVNeg->material()->E;\n",
"\tstrainRatio=1.0\n",
"\treturn positiveEnd ? (2.0 *strain*strainRatio/(1.0+strainRatio)) : (2.0*strain/(1.0+strainRatio))\n",
"end\n",
"\n",
"function updateStrain( axialStrain,E) # ?from where strain\n",
"\tstrain = axialStrain # redundant?\n",
"\tcurrentTransverseStrainSum=0.0 # ??? todo\n",
" linear=true\n",
" maxStrain=100000000000000000000;# ?? todo later change\n",
"\tif linear\n",
"\t\tif axialStrain > maxStrain\n",
"\t\t\tmaxStrain = axialStrain # remember this maximum for easy reference\n",
" end\n",
"\t\treturn stress(axialStrain,E)\n",
"\telse \n",
"\t\tif (axialStrain > maxStrain) # if new territory on the stress/strain curve\n",
"\t\t\tmaxStrain = axialStrain # remember this maximum for easy reference\n",
"\t\t\treturnStress = stress(axialStrain,E) # ??currentTransverseStrainSum\n",
"\t\t\tif (nu != 0.0) \n",
"\t\t\t\tstrainOffset = maxStrain-stress(axialStrain,E)/(_eHat*(1-nu)) # precalculate strain offset for when we back off\n",
"\t\t\telse \n",
" strainOffset = maxStrain-returnStress/E # precalculate strain offset for when we back off\n",
" end\n",
"\t\telse # backed off a non-linear material, therefore in linear region.\n",
"\t\t\trelativeStrain = axialStrain-strainOffset # treat the material as linear with a strain offset according to the maximum plastic deformation\n",
" if (nu != 0.0) \n",
"\t\t\t\treturnStress = stress(relativeStrain,E)\n",
"\t\t\telse \n",
"\t\t\t\treturnStress = E*relativeStrain\n",
" end\n",
" end\n",
"\t\treturn returnStress\n",
" end\n",
"end\n",
"\n",
"function stress( strain , E ) #end,transverseStrainSum, forceLinear){\n",
"\t# reference: http://www.colorado.edu/engineering/CAS/courses.d/Structures.d/IAST.Lect05.d/IAST.Lect05.pdf page 10\n",
"\t# if (isFailed(strain)) return 0.0f; //if a failure point is set and exceeded, we've broken!\n",
"\t# var E =setup.edges[0].stiffness; //todo change later to material ??\n",
"\t# var E=1000000;//todo change later to material ??\n",
"\t# var scaleFactor=1;\n",
" return E*strain;\n",
"\n",
"\t# # if (strain <= strainData[1] || linear || forceLinear){ //for compression/first segment and linear materials (forced or otherwise), simple calculation\n",
" \n",
" # if (nu==0.0) return E*strain;\n",
"\t\t# else return _eHat*((1-nu)*strain + nu*transverseStrainSum); \n",
"\t\t# else return eHat()*((1-nu)*strain + nu*transverseStrainSum); \n",
"\t# # }\n",
"\n",
"\t# //the non-linear feature with non-zero poissons ratio is currently experimental\n",
"\t# int DataCount = modelDataPoints();\n",
"\t# for (int i=2; i<DataCount; i++){ //go through each segment in the material model (skipping the first segment because it has already been handled.\n",
"\t# \tif (strain <= strainData[i] || i==DataCount-1){ //if in the segment ending with this point (or if this is the last point extrapolate out) \n",
"\t# \t\tfloat Perc = (strain-strainData[i-1])/(strainData[i]-strainData[i-1]);\n",
"\t# \t\tfloat basicStress = stressData[i-1] + Perc*(stressData[i]-stressData[i-1]);\n",
"\t# \t\tif (nu==0.0f) return basicStress;\n",
"\t# \t\telse { //accounting for volumetric effects\n",
"\t# \t\t\tfloat modulus = (stressData[i]-stressData[i-1])/(strainData[i]-strainData[i-1]);\n",
"\t# \t\t\tfloat modulusHat = modulus/((1-2*nu)*(1+nu));\n",
"\t# \t\t\tfloat effectiveStrain = basicStress/modulus; //this is the strain at which a simple linear stress strain line would hit this point at the definied modulus\n",
"\t# \t\t\tfloat effectiveTransverseStrainSum = transverseStrainSum*(effectiveStrain/strain);\n",
"\t# \t\t\treturn modulusHat*((1-nu)*effectiveStrain + nu*effectiveTransverseStrainSum);\n",
"\t# \t\t}\n",
"\t# \t}\n",
"\t# }\n",
"\n",
"\t# assert(false); //should never reach this point\n",
"\t# return 0.0f;\n",
"end \n"
]
},
{
"cell_type": "code",
"execution_count": 163,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"ToEulerAngles (generic function with 1 method)"
]
},
"execution_count": 163,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"function ToEulerAngles(q) # TODO I THINK WRONG\n",
" # roll (x-axis rotation)\n",
" sinr_cosp = (2 * (q[1] * q[2] + q[3] * q[4]) )== -0.0 ? 0.0 : (2 * (q[1] * q[2] + q[3] * q[4]) )\n",
" cosr_cosp = (1 - 2 * (q[2] * q[2] + q[3] * q[3]))== -0.0 ? 0.0 : (1 - 2 * (q[2] * q[2] + q[3] * q[3]))\n",
" \n",
" roll = atan(sinr_cosp, cosr_cosp)\n",
" \n",
"\n",
" # pitch (y-axis rotation)\n",
" sinp = 2 * (q[1] * q[3] - q[4] * q[2])\n",
" if (abs(sinp) >= 1)\n",
" pitch = copysign(π / 2, sinp) # use 90 degrees if out of range\n",
" else\n",
" pitch = asin(sinp)\n",
" end\n",
"\n",
" # yaw (z-axis rotation)\n",
" siny_cosp = 2 * (q[1] * q[4] + q[2] * q[3])\n",
" cosy_cosp = 1 - 2 * (q[3] * q[3] + q[4] * q[4])\n",
" yaw = atan(siny_cosp, cosy_cosp)\n",
" \n",
"\n",
" return [roll pitch yaw]\n",
"end\n",
"# ToEulerAngles(Quat(1.0,1.0,1.0,1.0))"
]
},
{
"cell_type": "code",
"execution_count": 164,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"timeStep (generic function with 1 method)"
]
},
"execution_count": 164,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"# http://klas-physics.googlecode.com/svn/trunk/src/general/Integrator.cpp (reference)\n",
"function timeStep(dt,node,static,currentTimeStep)\n",
"\tpreviousDt = dt\n",
"\tlinMom=copy(N_linMom[node])\n",
" angMom=copy(N_angMom[node])\n",
" orient=copy(N_orient[node])\n",
"\tpos=copy(N_currPosition[node])\n",
" \n",
" \n",
"\tif (dt == 0.0) \n",
"\t\treturn 0\n",
" end\n",
"\n",
"\tif(all(N_restrained_degrees_of_freedom[node] .>=1))\n",
"\t\t# pos = originalPosition() + ext->translation();\n",
"\t\t# orient = ext->rotationQuat();\n",
"\t\t# haltMotion();\n",
"\t\t# # pos=copy(N_position[node])\n",
"\t\t# # node.currPosition=pos.clone();\n",
"\t\t# # linMom = new THREE.Vector3(0,0,0);\n",
"\t\t# # angMom = new THREE.Vector3(0,0,0);\n",
"\t\t# # node.displacement={x:0,y:0,z:0};\n",
"\n",
"\t\t# node.orient=orient.clone();\n",
"\t\t# node.linMom=linMom.clone();\n",
"\t\t# node.angMom=angMom.clone();\n",
"\t\treturn 0\n",
" end\n",
" \n",
"\n",
"\t# Translation\n",
"\tcurForce = force(node,static,currentTimeStep)\n",
"\n",
"\t# var fricForce = curForce.clone();\n",
"\n",
"\t# if (isFloorEnabled()) floorForce(dt, &curForce); //floor force needs dt to calculate threshold to \"stop\" a slow voxel into static friction.\n",
"\n",
"\t# fricForce = curForce - fricForce;\n",
"\n",
"\t# assert(!(curForce.x != curForce.x) || !(curForce.y != curForce.y) || !(curForce.z != curForce.z)); //assert non QNAN\n",
"\tlinMom=linMom+curForce*dt\n",
" \n",
"\n",
"\tmassInverse=8e-6 # todo ?? later change\n",
"\ttranslate=linMom*(dt*massInverse) # ??massInverse\n",
"\n",
" # //\twe need to check for friction conditions here (after calculating the translation) and stop things accordingly\n",
"\t# if (isFloorEnabled() && floorPenetration() >= 0){ //we must catch a slowing voxel here since it all boils down to needing access to the dt of this timestep.\n",
"\t# \tdouble work = fricForce.x*translate.x + fricForce.y*translate.y; //F dot disp\n",
"\t# \tdouble hKe = 0.5*mat->_massInverse*(linMom.x*linMom.x + linMom.y*linMom.y); //horizontal kinetic energy\n",
"\n",
"\t# \tif(hKe + work <= 0) setFloorStaticFriction(true); //this checks for a change of direction according to the work-energy principle\n",
"\n",
"\t# \tif (isFloorStaticFriction()){ //if we're in a state of static friction, zero out all horizontal motion\n",
"\t# \t\tlinMom.x = linMom.y = 0;\n",
"\t# \t\ttranslate.x = translate.y = 0;\n",
"\t# \t}\n",
"\t# }\n",
"\t# else setFloorStaticFriction(false);\n",
" pos=pos+translate\n",
" \n",
" \n",
" N_currPosition[node]=copy(pos)\n",
" N_displacement[node]=N_displacement[node]+translate\n",
"\n",
"\t# Rotation\n",
"\tcurMoment = moment(node)\n",
" angMom=angMom+curMoment*dt\n",
" \n",
"\n",
" \n",
"\tmomentInertiaInverse=1.0 # todo ?? later change\n",
" # orient=orient.*FromRotationVector(copy(angMom)*(dt*momentInertiaInverse))\n",
" \n",
" temp=FromRotationVector(copy(angMom)*(dt*momentInertiaInverse))\n",
" \n",
" orient=[orient[1]*temp[1] orient[2]*temp[2] orient[3]*temp[3] orient[4]*temp[4]]\n",
" \n",
"\t# orient.multiply(FromRotationVector(angMom.clone().multiplyScalar((dt*momentInertiaInverse)))) # tupdate the orientation //momentInertiaInverse\n",
"\n",
" N_orient[node]=copy(orient)\n",
" \n",
"\t\n",
"\teul = ToEulerAngles(orient) # TODO I THINK WRONG\n",
" \n",
" N_angle[node]=copy(eul)\n",
"\n",
" N_linMom[node]=copy(linMom)\n",
" N_angMom[node]=copy(angMom)\n",
" \n",
" \n",
"\t\n",
"\t# if (ext){//?? todo fix \n",
"\t# \tvar size = 1;//change\n",
"\t# \tif (ext->isFixed(X_TRANSLATE)) {pos.x = ix*size + ext->translation().x; linMom.x=0;}\n",
"\t# \tif (ext->isFixed(Y_TRANSLATE)) {pos.y = iy*size + ext->translation().y; linMom.y=0;}\n",
"\t# \tif (ext->isFixed(Z_TRANSLATE)) {pos.z = iz*size + ext->translation().z; linMom.z=0;}\n",
"\t# \tif (ext->isFixedAnyRotation()){ //if any rotation fixed, all are fixed\n",
"\t# \t\tif (ext->isFixedAllRotation()){\n",
"\t# \t\t\torient = ext->rotationQuat();\n",
"\t# \t\t\tangMom = Vec3D<double>();\n",
"\t# \t\t}\n",
"\t# \t\telse { //partial fixes: slow!\n",
"\t# \t\t\tVec3D<double> tmpRotVec = orient.ToRotationVector();\n",
"\t# \t\t\tif (ext->isFixed(X_ROTATE)){ tmpRotVec.x=0; angMom.x=0;}\n",
"\t# \t\t\tif (ext->isFixed(Y_ROTATE)){ tmpRotVec.y=0; angMom.y=0;}\n",
"\t# \t\t\tif (ext->isFixed(Z_ROTATE)){ tmpRotVec.z=0; angMom.z=0;}\n",
"\t# \t\t\torient.FromRotationVector(tmpRotVec);\n",
"\t# \t\t}\n",
"\t# \t}\n",
"\t# }\n",
"\n",
"\t# poissonsStrainInvalid = true;\n",
"end"
]
},
{
"cell_type": "code",
"execution_count": 165,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"moment (generic function with 1 method)"
]
},
"execution_count": 165,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"function force(node,static,currentTimeStep) \n",
"\t# forces from internal bonds\n",
"\ttotalForce=[0 0 0]\n",
"\t# new THREE.Vector3(node.force.x,node.force.y,node.force.z);\n",
"\t# todo \n",
" \n",
" \n",
" \n",
"\ttotalForce=totalForce+N_intForce[node]\n",
"\n",
"\t# for (int i=0; i<6; i++){ \n",
"\t# \tif (links[i]) totalForce += links[i]->force(isNegative((linkDirection)i)); # total force in LCS\n",
"\t# }\n",
"\ttotalForce = RotateVec3D(N_orient[node],totalForce); # from local to global coordinates\n",
" \n",
"\n",
"\t# assert(!(totalForce.x != totalForce.x) || !(totalForce.y != totalForce.y) || !(totalForce.z != totalForce.z)); //assert non QNAN\n",
"\n",
"\t# other forces\n",
"\tif(static)\n",
" totalForce=totalForce+N_force[node]\n",
"\t# }else if(currentTimeStep<50){\n",
"\t# \ttotalForce.add(new THREE.Vector3(node.force.x,node.force.y,node.force.z));\n",
"\telse\n",
"\t\t# var ex=0.1;\n",
"\t\t# if(node.force.y!=0){\n",
"\t\t# \tvar f=400*Math.sin(currentTimeStep*ex);\n",
"\t\t# \ttotalForce.add(new THREE.Vector3(0,f,0));\n",
"\t\t\t\n",
"\t\t# }\n",
"\t\tx=N_position[node][3]\n",
"\t\tt=currentTimeStep\n",
"\t\twave=getForce(x,t)\n",
" totalForce=totalForce+[0 wave 0]\t\t\n",
" end\n",
" \n",
"\n",
"\t# if (externalExists()) totalForce += external()->force(); //external forces\n",
"\t# totalForce -= velocity()*mat->globalDampingTranslateC(); //global damping f-cv\n",
"\t# totalForce.z += mat->gravityForce(); //gravity, according to f=mg\n",
"\n",
"\t# if (isCollisionsEnabled()){\n",
"\t# \tfor (std::vector<CVX_Collision*>::iterator it=colWatch->begin(); it!=colWatch->end(); it++){\n",
"\t# \t\ttotalForce -= (*it)->contactForce(this);\n",
"\t# \t}\n",
"\t# }\n",
"\t# todo make internal forces 0 again\n",
" N_intForce[node]=[0 0 0] # do i really need it?\n",
" \n",
"\t# node.force.x=0;\n",
"\t# node.force.y=0;\n",
"\t# node.force.z=0;\n",
" \n",
" \n",
"\treturn totalForce\n",
"end\n",
"\n",
"function moment(node) \n",
"\t#moments from internal bonds\n",
"\ttotalMoment=[0 0 0]\n",
"\t# for (int i=0; i<6; i++){ \n",
"\t# \tif (links[i]) totalMoment += links[i]->moment(isNegative((linkDirection)i)); //total force in LCS\n",
"\t# }\n",
"\n",
" totalMoment=totalMoment+N_intMoment[node]\n",
" \n",
"\ttotalMoment = RotateVec3D(N_orient[node],totalMoment);\n",
" \n",
" totalMoment=totalMoment+N_moment[node]\n",
"\n",
"\t#other moments\n",
"\t# if (externalExists()) totalMoment += external()->moment(); //external moments\n",
"\t# totalMoment -= angularVelocity()*mat->globalDampingRotateC(); //global damping\n",
"\t\n",
" N_intMoment[node]=[0 0 0] # do i really need it?\n",
" \n",
"\treturn totalMoment\n",
"end"
]
},
{
"cell_type": "code",
"execution_count": 166,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"updateDataAndSave (generic function with 1 method)"
]
},
"execution_count": 166,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"function updateDataAndSave(setup,fileName)\n",
" nodes = setup[\"nodes\"]\n",
" edges = setup[\"edges\"]\n",
" voxCount=size(nodes)[1]\n",
" linkCount=size(edges)[1]\n",
"\n",
" i=1\n",
"\tfor edge in edges\n",
" edge[\"stress\"]=E_stress[i]\n",
" i=i+1\n",
"\n",
" end\n",
" \n",
" \n",
" i=1 \n",
"\tfor node in nodes\n",
" node[\"displacement\"][\"x\"]=N_displacement[i][1]\n",
" node[\"displacement\"][\"y\"]=N_displacement[i][2]\n",
" node[\"displacement\"][\"z\"]=N_displacement[i][3]\n",
" \n",
" node[\"angle\"][\"x\"]=N_angle[i][1]\n",
" node[\"angle\"][\"y\"]=N_angle[i][2]\n",
" node[\"angle\"][\"z\"]=N_angle[i][3]\n",
" i=i+1\n",
"\n",
" end\n",
" \n",
" # pass data as a json string (how it shall be displayed in a file)\n",
" stringdata = JSON.json(setup)\n",
"\n",
" # write the file with the stringdata variable information\n",
" open(fileName, \"w\") do f\n",
" write(f, stringdata)\n",
" end\n",
" \n",
"end"
]
},
{
"cell_type": "code",
"execution_count": 169,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"Dict{String,Any} with 6 entries:\n",
" \"nodes\" => Any[Dict{String,Any}(\"degrees_of_freedom\"=>Any[0, 1, 2, 3, 4, …\n",
" \"bar\" => false\n",
" \"animation\" => Dict{String,Any}(\"speed\"=>3,\"exaggeration\"=>100,\"showDisplaceme…\n",
" \"viz\" => Dict{String,Any}(\"colorMap\"=>0,\"colorMaps\"=>Any[],\"maxStress\"=…\n",
" \"edges\" => Any[Dict{String,Any}(\"currentRestLength\"=>0,\"source\"=>0,\"area\"…\n",
" \"ndofs\" => 18"
]
},
"execution_count": 169,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"setup = Dict()\n",
"open(\"../json/setupValid2.json\", \"r\") do f\n",
" global setup\n",
" dicttxt = String(read(f)) # file information to string\n",
" setup=JSON.parse(dicttxt) # parse and transform data\n",
"end\n",
"\n",
"setup=setup[\"setup\"]"
]
},
{
"cell_type": "code",
"execution_count": 173,
"metadata": {},
"outputs": [
{
"ename": "BoundsError",
"evalue": "BoundsError",
"output_type": "error",
"traceback": [
"BoundsError",
"",
"Stacktrace:",
" [1] getindex at .\\number.jl:78 [inlined]",
" [2] RotateVec3D(::Array{Float64,2}, ::Array{Float64,2}) at .\\In[154]:13",
" [3] orientLink(::Int64) at .\\In[172]:13",
" [4] updateForces(::Dict{String,Any}, ::Int64, ::Bool) at .\\In[152]:25",
" [5] doTimeStep(::Dict{String,Any}, ::Float64, ::Bool, ::Int64, ::Int64) at .\\In[151]:27",
" [6] simulateParallel(::Dict{String,Any}, ::Int64, ::Float64, ::Bool, ::Int64) at .\\In[149]:5",
" [7] top-level scope at util.jl:289",
" [8] top-level scope at In[173]:50"
]
}
],
"source": [
"\n",
"\n",
"\n",
"############# nodes\n",
"N_position=[]\n",
"N_degrees_of_freedom=[]\n",
"N_restrained_degrees_of_freedom=[]\n",
"N_displacement=[]\n",
"N_angle=[]\n",
"N_currPosition=[]\n",
"N_linMom=[]\n",
"N_angMom=[]\n",
"N_intForce=[]\n",
"N_intMoment=[]\n",
"N_moment=[]\n",
"N_posTimeSteps=[]\n",
"N_angTimeSteps=[]\n",
"N_force=[]\n",
"N_orient=[]\n",
"\n",
"############# edges\n",
"E_source=[]\n",
"E_target=[]\n",
"E_area=[]\n",
"E_density=[]\n",
"E_stiffness=[]\n",
"E_stress=[]\n",
"E_axis=[]\n",
"E_currentRestLength=[]\n",
"E_pos2=[]\n",
"E_angle1v=[]\n",
"E_angle2v=[]\n",
"E_angle1=[]\n",
"E_angle2=[]\n",
"E_currentTransverseStrainSum=[]\n",
"E_stressTimeSteps=[]\n",
"\n",
"########\n",
"voxCount=0\n",
"linkCount=0\n",
"nodes = setup[\"nodes\"]\n",
"edges = setup[\"edges\"]\n",
"voxCount=size(nodes)[1]\n",
"linkCount=size(edges)[1]\n",
"strain =0 #todooo moveeee\n",
"\n",
"dt=0.0251646\n",
"numTimeSteps =1\n",
"latticeSize =1\n",
"t=@timed simulateParallel(setup,numTimeSteps,dt,true,10)\n",
"time=t[2]\n",
"println(\"ran latticeSize $latticeSize with $voxCount voxels and $linkCount edges for $numTimeSteps time steps took $time seconds\")\n",
"N_displacement"
]
},
{
"cell_type": "code",
"execution_count": 171,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"4486"
]
},
"execution_count": 171,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"save=true\n",
"if save\n",
" updateDataAndSave(setup,\"../json/trialJuliaParallel.json\")\n",
"end"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
"display_name": "Julia 1.2.0",
"language": "julia",
"name": "julia-1.2"
},
"language_info": {
"file_extension": ".jl",
"mimetype": "application/julia",
"name": "julia",
"version": "1.2.0"
}
},
"nbformat": 4,
"nbformat_minor": 4
}