public override void UpdateState(double[] incrementalNodeDisplacements)
{
//displacementsOfCurrentIncrement.Add(new Vector(incrementalNodeDisplacements));
//lastDisplacements.CopyTo(currentDisplacements.Data, 0);
//currentDisplacements.Add(displacementsOfCurrentIncrement);
//this.currentDisplacements.addIntoThis(this.lastDisplacements, incrementalNodeDisplacements);
currentDisplacements.CopyFrom(lastDisplacements);
currentDisplacements.AddIntoThis(incrementalNodeDisplacements);
displacementsOfCurrentIncrement.CopyFrom(incrementalNodeDisplacements);
var incrementalRotationsA = new double[AXIS_COUNT];
var incrementalRotationsB = new double[AXIS_COUNT];
incrementalRotationsA[0] = displacementsOfCurrentIncrement[3];
incrementalRotationsA[1] = displacementsOfCurrentIncrement[4];
incrementalRotationsA[2] = displacementsOfCurrentIncrement[5];
incrementalRotationsB[0] = displacementsOfCurrentIncrement[9];
incrementalRotationsB[1] = displacementsOfCurrentIncrement[10];
incrementalRotationsB[2] = displacementsOfCurrentIncrement[11];
double[] qA = quaternionLastNodeA.VectorPart.Copy();
double[] qB = quaternionLastNodeB.VectorPart.Copy();
this.quaternionCurrentNodeA = new Quaternion(quaternionLastNodeA.ScalarPart, qA);
this.quaternionCurrentNodeB = new Quaternion(quaternionLastNodeB.ScalarPart, qB);
this.quaternionCurrentNodeA.ApplyIncrementalRotation(incrementalRotationsA);
this.quaternionCurrentNodeB.ApplyIncrementalRotation(incrementalRotationsB);
double scalarA = this.quaternionCurrentNodeA.ScalarPart;
double scalarB = this.quaternionCurrentNodeB.ScalarPart;
var vectorPartA = this.quaternionCurrentNodeA.VectorPart;
var vectorPartB = this.quaternionCurrentNodeB.VectorPart;
double[] sumOfVectorParts = vectorPartA.Copy();
sumOfVectorParts.AddIntoThis(vectorPartB);
double sumOfVectorPartsNorm = sumOfVectorParts.Norm2();
double scalarPartDifference = 0.5 * Math.Sqrt(((scalarA + scalarB) * (scalarA + scalarB)) + (sumOfVectorPartsNorm * sumOfVectorPartsNorm));
double meanRotationScalarPart = (0.5 * (scalarA + scalarB)) / scalarPartDifference;
double[] meanRotationVectorPart = vectorPartA.Copy();
meanRotationVectorPart.AddIntoThis(vectorPartB);
meanRotationVectorPart.ScaleIntoThis(1d / (2d * scalarPartDifference));
double[] vectorPartDifference = vectorPartB.Copy();
vectorPartDifference.ScaleIntoThis(scalarA);
vectorPartDifference.AddIntoThis(vectorPartA.Scale(-scalarB));
//vectorPartDifference.doPointwise(vectorPartA, DoubleBinaryOps.alphaPlusScaledBeta(-scalarB));
vectorPartDifference.AddIntoThis(vectorPartA.CrossProduct(vectorPartB));
vectorPartDifference.ScaleIntoThis(1d / (2d * scalarPartDifference));
var meanRotationQuaternion = Quaternion.CreateFromIndividualParts(meanRotationScalarPart, meanRotationVectorPart);
this.currentRotationMatrix = meanRotationQuaternion.GetRotationMatrix();
this.CalculateUpdatedBeamAxis();
this.UpdateRotationMatrix();
this.UpdateNaturalDeformations(vectorPartDifference);
//SaveGeometryState();
}
/**
* Rotates the quaternion to a new quaternion by an incremental rotation given in vector form.
*
* @param incrementalRotation
* The incremental rotation
* @return The rotated quaternion
*/
public Quaternion ApplyIncrementalRotationToNew(double[] incrementalRotation)
{
Debug.Assert(incrementalRotation.Length == VECTOR_COMPONENT_COUNT);
double[] vectorPartIncrement = incrementalRotation.Copy();
vectorPartIncrement.ScaleIntoThis(0.5);
double squareRootArgument = 1.0 - vectorPartIncrement.DotProduct(vectorPartIncrement);
//Preconditions.checkArgument(squareRootArgument > 0.0, "Very large rotation increment applied");
double scalarPartIncrement = Math.Sqrt(squareRootArgument);
double updatedScalarPart = (scalarPartIncrement * this.scalarPart) - vectorPartIncrement.DotProduct(this.vectorPart);
//TODO: Is the following correct? Vector updatedVectorPart = this.vectorPart; does not copy the original vector.
//TODO: The next part creates a new Quaternion (using legacy linear algebra classes), but the existing ones is not copied as per the author's intention
double[] updatedVectorPart = this.vectorPart;
updatedVectorPart.ScaleIntoThis(scalarPartIncrement);
updatedVectorPart.AddIntoThis(vectorPartIncrement.Scale(scalarPart));
double[] incrementCrossVectorPart = vectorPartIncrement.CrossProduct(this.vectorPart);
updatedVectorPart.AddIntoThis(incrementCrossVectorPart);
return(new Quaternion(updatedScalarPart, updatedVectorPart));
}
/**
* Rotates the quaternion by an incremental rotation given in vector form.
*
* @param incrementalRotation
* The incremental rotation
*/
public void ApplyIncrementalRotation(double[] incrementalRotation)
{
Debug.Assert(incrementalRotation.Length == VECTOR_COMPONENT_COUNT);
double[] vectorPartIncrement = incrementalRotation.Copy();
//var vectorPartIncrement = new Vector(VECTOR_COMPONENT_COUNT);
//incrementalRotation.CopyTo(vectorPartIncrement.Data, 0);
vectorPartIncrement.ScaleIntoThis(0.5);
double squareRootArgument = 1.0 - vectorPartIncrement.DotProduct(vectorPartIncrement);
//Preconditions.checkArgument(squareRootArgument > 0.0, "Very large rotation increment applied");
double scalarPartIncrement = Math.Sqrt(squareRootArgument);
double updatedScalarPart = (scalarPartIncrement * this.scalarPart) - vectorPartIncrement.DotProduct(this.vectorPart);
//TODO: Is the following correct? Vector updatedVectorPart = this.vectorPart; does not copy the original vector.
//TODO: Why not use this.vectorPart for all the next?
double[] updatedVectorPart = this.vectorPart;
updatedVectorPart.ScaleIntoThis(scalarPartIncrement);
updatedVectorPart.AddIntoThis(vectorPartIncrement.Scale(scalarPart));
double[] incrementCrossVectorPart = vectorPartIncrement.CrossProduct(this.vectorPart);
updatedVectorPart.AddIntoThis(incrementCrossVectorPart);
this.scalarPart = updatedScalarPart;
this.vectorPart.CopyFrom(updatedVectorPart); //TODO: This does nothing. updatedVectorPart is a reference to the same memory as this.vectorPart
}
public override void UpdateState(double[] incrementalNodeDisplacements)
{
currentDisplacements.CopyFrom(lastDisplacements);
currentDisplacements.AddIntoThis(incrementalNodeDisplacements);
displacementsOfCurrentIncrement.CopyFrom(incrementalNodeDisplacements);
double currentDisplacementX_A = currentDisplacements[0];
double currentlDisplacementY_A = currentDisplacements[1];
double currentRotationZ_A = currentDisplacements[2];
double currentDisplacementX_B = currentDisplacements[3];
double currentDisplacementY_B = currentDisplacements[4];
double currentRotationZ_Β = currentDisplacements[5];
double dX = ((nodes[1].X - nodes[0].X) + currentDisplacementX_B) - currentDisplacementX_A;
double dY = ((nodes[1].Y - nodes[0].Y) + currentDisplacementY_B) - currentlDisplacementY_A;
this.currentLength = Math.Sqrt((dX * dX) + (dY * dY));
double axisAngle = 0d;
if ((dY == 0) && (dX == currentLength))
{
axisAngle = 0d;
}
else
if ((dY == 0) && (dX == -currentLength))
{
axisAngle = Math.PI;
}
else
{
axisAngle = 2.0 * Math.Atan((currentLength - dX) / dY);
}
double initialdX = nodes[1].X - nodes[0].X;
double initialdY = nodes[1].Y - nodes[0].Y;
double initialLength = Math.Sqrt((initialdX * initialdX) + (initialdY * initialdY));
double axisAngleInitial = 0d;
if ((initialdY == 0) && (initialdX == initialLength))
{
axisAngleInitial = 0d;
}
else
if ((initialdY == 0) && (initialdX == -initialLength))
{
axisAngleInitial = Math.PI;
}
else
{
axisAngleInitial = 2.0 * Math.Atan((initialLength - dX) / dY);
}
double symmetricAngle = currentRotationZ_Β - currentRotationZ_A;
double antiSymmetricAngle = currentRotationZ_Β + currentRotationZ_A - 2.0 * (axisAngle - axisAngleInitial);
currentRotationMatrix = RotationMatrix.CalculateRotationMatrixBeam2D(axisAngle);
double extension = this.currentLength - this.initialLength;
this.naturalDeformations[0] = extension;
this.naturalDeformations[1] = symmetricAngle;
this.naturalDeformations[2] = antiSymmetricAngle;
}