//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

}