//sub force calculation types...
void CVXS_Bond::CalcLinForce() //get bond forces given positions, angles, and stiffnesses...
{
Vec3D<double> CurXRelPos(pVox2->GetCurPosHighAccuracy() - pVox1->GetCurPosHighAccuracy()); //digit truncation happens here...
CQuat<double> CurXAng1(pVox1->GetCurAngleHighAccuracy());
CQuat<double> CurXAng2(pVox2->GetCurAngleHighAccuracy());
ToXDirBond(&CurXRelPos);
ToXDirBond(&CurXAng1);
ToXDirBond(&CurXAng2);
Vec3D<double> Ang1AlignedRelPos(CurXAng1.RotateVec3DInv(CurXRelPos)); //undo current voxel rotation to put in line with original bond according to Angle 1
CQuat<double> NewAng2(CurXAng1.Conjugate()*CurXAng2);
bool ChangedSaState = false;
vfloat SmallTurn = (abs(Ang1AlignedRelPos.z)+abs(Ang1AlignedRelPos.y))/Ang1AlignedRelPos.x;
if (!SmallAngle && NewAng2.IsSmallAngle() && SmallTurn < SA_BOND_BEND_RAD){ SmallAngle = true; ChangedSaState=true;}
else if (SmallAngle && !NewAng2.IsSmallishAngle() && SmallTurn > VEC3D_HYSTERESIS_FACTOR*SA_BOND_BEND_RAD){SmallAngle = false; ChangedSaState=true;}
double NomDistance = (pVox1->GetCurScale().x + pVox2->GetCurScale().x)*0.5; //nominal distance between voxels
CQuat<> TotalRot;
if (SmallAngle) { //Align so Angle1 is all zeros
_Angle1 = Vec3D<>(0,0,0);
_Angle2 = NewAng2.ToRotationVector();
Ang1AlignedRelPos.x -= NomDistance; //only valid for small angles
_Pos2 = Ang1AlignedRelPos;
TotalRot = CurXAng1.Conjugate();
}
else { //Large angle. Align so that Pos2.y, Pos2.z are zero.
CQuat<double> Pos2AlignedRotAng;
Pos2AlignedRotAng.FromAngleToPosX(Ang1AlignedRelPos); //get the angle to align this with the X axis
TotalRot = Pos2AlignedRotAng * CurXAng1.Conjugate();
vfloat Length = CurXRelPos.Length(); //Ang1AlignedRelPos.x<0 ? -Ang1AlignedRelPos.Length() : Ang1AlignedRelPos.Length();
_Pos2 = Vec3D<>(Length - NomDistance, 0, 0); //Small angle optimization target!!
// Vec3D<> Pos2a = Pos2AlignedRotAng.RotateVec3D(Ang1AlignedRelPos); //high performance (but slow version) for special cases. should never crop up. (i.e. 1dof sim dragging voxel past each other)
// _Pos2 = Vec3D<>(Pos2a.x - NomDistance, 0, 0);
_Angle1 = Pos2AlignedRotAng.ToRotationVector(); //these are currently a source of error of up to 1e-6
_Angle2 = (TotalRot * CurXAng2).ToRotationVector();
}
UpdateBondStrain(_Pos2.x/L.x); //updates the bond parameters (yielded, broken, stress...) based on the current Strain
//Beam equations! (all terms here, even though some are sero for small angle and large angle (negligible perfoprmance penalty)
Force1 = Vec3D<> ( -CurStress*L.y*L.z, -b1z*_Pos2.y + b2z*(_Angle1.z + _Angle2.z), -b1y*_Pos2.z - b2y*(_Angle1.y + _Angle2.y)); //Use Curstress instead of -a1*Pos2.x to account for non-linear deformation
Force2 = -Force1;
Moment1 = Vec3D<> ( a2*(_Angle1.x - _Angle2.x), b2z*_Pos2.z + b3y*(2*_Angle1.y + _Angle2.y), -b2y*_Pos2.y + b3z*(2*_Angle1.z + _Angle2.z));
Moment2 = Vec3D<> ( a2*(_Angle2.x - _Angle1.x), b2z*_Pos2.z + b3y*(_Angle1.y + 2*_Angle2.y), -b2y*_Pos2.y + b3z*(_Angle1.z + 2*_Angle2.z));
if (p_Sim->StatToCalc & CALCSTAT_STRAINE) StrainEnergy = CalcStrainEnergy(); //depends on Force1, Force2, Moment1, Moment2 being set!
if (!ChangedSaState) AddDampForces();
//Unrotate back to global coordinate system
Force1 = TotalRot.RotateVec3DInv(Force1);
if (!HomogenousBond) Force2 = TotalRot.RotateVec3DInv(Force2); //could reduce for homogenous...
Moment1 = TotalRot.RotateVec3DInv(Moment1);
Moment2 = TotalRot.RotateVec3DInv(Moment2);
ToOrigDirBond(&Force1);
if (HomogenousBond) Force2 = -Force1;
else ToOrigDirBond(&Force2); //Added
ToOrigDirBond(&Moment1);
ToOrigDirBond(&Moment2);
}
//sub force calculation types...
void CVXS_BondInternal::CalcLinForce() //get bond forces given positions, angles, and stiffnesses...
{
Vec3D<double> CurXRelPos(pVox2->GetCurPosHighAccuracy() - pVox1->GetCurPosHighAccuracy()); //digit truncation happens here...
CQuat<double> CurXAng1(pVox1->GetCurAngleHighAccuracy());
CQuat<double> CurXAng2(pVox2->GetCurAngleHighAccuracy());
ToXDirBond(&CurXRelPos);
ToXDirBond(&CurXAng1);
ToXDirBond(&CurXAng2);
Vec3D<double> Ang1AlignedRelPos(CurXAng1.RotateVec3DInv(CurXRelPos)); //undo current voxel rotation to put in line with original bond according to Angle 1
CQuat<double> NewAng2(CurXAng1.Conjugate()*CurXAng2);
//todo: lump into stress calculations
double NomDistance;
if (p_Sim->IsFeatureEnabled(VXSFEAT_VOLUME_EFFECTS)) NomDistance = L.x;
else NomDistance = (pVox1->GetCurScale() + pVox2->GetCurScale())*0.5; //nominal distance between voxels
bool ChangedSaState = false;
vfloat SmallTurn = (abs(Ang1AlignedRelPos.z)+abs(Ang1AlignedRelPos.y))/Ang1AlignedRelPos.x;
vfloat ExtendPerc = Ang1AlignedRelPos.x/NomDistance;
if (!SmallAngle && NewAng2.IsSmallAngle() && SmallTurn < SA_BOND_BEND_RAD && ExtendPerc < SA_BOND_EXT_PERC){ SmallAngle = true; ChangedSaState=true;}
else if (SmallAngle && (!NewAng2.IsSmallishAngle() || SmallTurn > VEC3D_HYSTERESIS_FACTOR*SA_BOND_BEND_RAD || ExtendPerc > VEC3D_HYSTERESIS_FACTOR*SA_BOND_EXT_PERC)){SmallAngle = false; ChangedSaState=true;}
CQuat<> TotalRot;
//Shear stuff!
vfloat ShearStrainY=0;
vfloat ShearStrainZ=0;
if (SmallAngle) { //Align so Angle1 is all zeros
_Angle1 = Vec3D<>(0,0,0);
_Angle2 = NewAng2.ToRotationVector();
Ang1AlignedRelPos.x -= NomDistance; //only valid for small angles
_Pos2 = Ang1AlignedRelPos;
TotalRot = CurXAng1.Conjugate();
ShearStrainY =-_Pos2.y/L.x; //delta Y/l
ShearStrainZ =-_Pos2.z/L.x; //delta Z/l
//vfloat Phi = 12*E*I/(G*A*L*L);
//ShearStrainY = -(_Pos2.y/L.x + _Angle2.z*Phi/2);
}
else { //Large angle. Align so that Pos2.y, Pos2.z are zero.
CQuat<double> Pos2AlignedRotAng;
Pos2AlignedRotAng.FromAngleToPosX(Ang1AlignedRelPos); //get the angle to align this with the X axis
TotalRot = Pos2AlignedRotAng * CurXAng1.Conjugate();
vfloat Length = CurXRelPos.Length(); //Ang1AlignedRelPos.x<0 ? -Ang1AlignedRelPos.Length() : Ang1AlignedRelPos.Length();
_Pos2 = Vec3D<>(Length - NomDistance, 0, 0); //Small angle optimization target!!
// Vec3D<> Pos2a = Pos2AlignedRotAng.RotateVec3D(Ang1AlignedRelPos); //high performance (but slow version) for special cases. should never crop up. (i.e. 1dof sim dragging voxel past each other)
// _Pos2 = Vec3D<>(Pos2a.x - NomDistance, 0, 0);
_Angle1 = Pos2AlignedRotAng.ToRotationVector(); //these are currently a source of error of up to 1e-6
_Angle2 = (TotalRot * CurXAng2).ToRotationVector();
// ShearStrainY = tan(_Angle2.z-_Angle1.z); //tan theta
// ShearStrainZ = -tan(_Angle2.y-_Angle1.y); //tan theta
ShearStrainY = -tan(_Angle1.z); //tan theta
ShearStrainZ = tan(_Angle1.y); //tan theta
}
UpdateBondStrain(_Pos2.x/L.x); //updates the bond parameters (yielded, broken, stress...) based on the current Strain
//Beam equations! (all terms here, even though some are zero for small angle and large angle (negligible performance penalty)
// if (p_Sim->IsFeatureEnabled(VXSFEAT_VOLUME_EFFECTS))
// Force1 = Vec3D<> ( -CurStress*(CSArea1+CSArea2)/2, -b1z*_Pos2.y + b2z*(_Angle1.z + _Angle2.z) - G*A*ShearStrainY, -b1y*_Pos2.z - b2y*(_Angle1.y + _Angle2.y)/*+ G*A*ShearStrainZ*/); //Use Curstress instead of -a1*Pos2.x to account for non-linear deformation
// else
/// Force1 = Vec3D<> ( -CurStress*(CSArea1+CSArea2)/2, -b1z*_Pos2.y + b2z*(_Angle1.z + _Angle2.z), -b1y*_Pos2.z - b2y*(_Angle1.y + _Angle2.y)); //Use Curstress instead of -a1*Pos2.x to account for non-linear deformation
Force1 = Vec3D<> ( CurStress*(CSArea1+CSArea2)/2, b1z*_Pos2.y - b2z*(_Angle1.z + _Angle2.z), b1y*_Pos2.z + b2y*(_Angle1.y + _Angle2.y)); //Use Curstress instead of -a1*Pos2.x to account for non-linear deformation
Force2 = -Force1;
#ifdef DEBUG
AxialForce1 = Vec3D<>(Force1.x, 0, 0);
AxialForce2 = Vec3D<>(Force2.x, 0, 0);
BendingForce1 = Vec3D<>(0, Force1.y, Force1.z);
BendingForce2 = Vec3D<>(0, Force2.y, Force2.z);
ShearForce1 = ShearForce2 = Vec3D<>(0,0,0);
if (p_Sim->IsFeatureEnabled(VXSFEAT_VOLUME_EFFECTS)){
vfloat CA = G*(CSArea1+CSArea2)/2;
//vfloat CA = G*A;
Force1.y += CA*ShearStrainY;
Force2.y -= CA*ShearStrainY;
Force1.z += CA*ShearStrainZ;
Force2.z -= CA*ShearStrainZ;
ShearForce1 = Vec3D<>(0, CA*ShearStrainY, CA*ShearStrainZ);
ShearForce2 = Vec3D<>(0, -CA*ShearStrainY, -CA*ShearStrainZ);
}
#endif
Moment1 = Vec3D<> ( a2*(_Angle1.x - _Angle2.x), b2z*_Pos2.z + b3y*(2*_Angle1.y + _Angle2.y), -b2y*_Pos2.y + b3z*(2*_Angle1.z + _Angle2.z));
Moment2 = Vec3D<> ( a2*(_Angle2.x - _Angle1.x), b2z*_Pos2.z + b3y*(_Angle1.y + 2*_Angle2.y), -b2y*_Pos2.y + b3z*(_Angle1.z + 2*_Angle2.z));
if (p_Sim->StatToCalc & CALCSTAT_STRAINE) StrainEnergy = CalcStrainEnergy(); //depends on Force1, Force2, Moment1, Moment2 being set!
if (!ChangedSaState) AddDampForces();
//Unrotate back to global coordinate system:
//!!possible optimization: Do this after summing forces for a voxel!
Force1 = TotalRot.RotateVec3DInv(Force1);
if (!HomogenousBond) Force2 = TotalRot.RotateVec3DInv(Force2);
Moment1 = TotalRot.RotateVec3DInv(Moment1);
Moment2 = TotalRot.RotateVec3DInv(Moment2);
ToOrigDirBond(&Force1);
if (HomogenousBond) Force2 = -Force1;
else ToOrigDirBond(&Force2); //Added
ToOrigDirBond(&Moment1);
ToOrigDirBond(&Moment2);
#ifdef DEBUG
AxialForce1 = TotalRot.RotateVec3DInv(AxialForce1);
AxialForce2 = TotalRot.RotateVec3DInv(AxialForce2);
ShearForce1 = TotalRot.RotateVec3DInv(ShearForce1);
ShearForce2 = TotalRot.RotateVec3DInv(ShearForce2);
BendingForce1 = TotalRot.RotateVec3DInv(BendingForce1);
BendingForce2 = TotalRot.RotateVec3DInv(BendingForce2);
ToOrigDirBond(&AxialForce1);
ToOrigDirBond(&AxialForce2);
ToOrigDirBond(&ShearForce1);
ToOrigDirBond(&ShearForce2);
ToOrigDirBond(&BendingForce1);
ToOrigDirBond(&BendingForce2);
#endif
}
