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; }