void BeamElement::CalculateLocalNodalStress(Vector& Stress)
{
Matrix Rotation;
Matrix LocalMatrix;
array_1d<double, 12 > CurrentDisplacement;
array_1d<double, 12 > LocalDisplacement;
Vector LocalBody = ZeroVector(12);
Vector GlobalBody = ZeroVector(12);
Rotation.resize(12,12, false);
Stress.resize(12, false);
CurrentDisplacement(0) = GetGeometry()[0].GetSolutionStepValue(DISPLACEMENT_X);
CurrentDisplacement(1) = GetGeometry()[0].GetSolutionStepValue(DISPLACEMENT_Y);
CurrentDisplacement(2) = GetGeometry()[0].GetSolutionStepValue(DISPLACEMENT_Z);
CurrentDisplacement(3) = GetGeometry()[0].GetSolutionStepValue(ROTATION_X);
CurrentDisplacement(4) = GetGeometry()[0].GetSolutionStepValue(ROTATION_Y);
CurrentDisplacement(5) = GetGeometry()[0].GetSolutionStepValue(ROTATION_Z);
CurrentDisplacement(6) = GetGeometry()[1].GetSolutionStepValue(DISPLACEMENT_X);
CurrentDisplacement(7) = GetGeometry()[1].GetSolutionStepValue(DISPLACEMENT_Y);
CurrentDisplacement(8) = GetGeometry()[1].GetSolutionStepValue(DISPLACEMENT_Z);
CurrentDisplacement(9) = GetGeometry()[1].GetSolutionStepValue(ROTATION_X);
CurrentDisplacement(10) = GetGeometry()[1].GetSolutionStepValue(ROTATION_Y);
CurrentDisplacement(11) = GetGeometry()[1].GetSolutionStepValue(ROTATION_Z);
CalculateTransformationMatrix(Rotation);
CalculateLocalMatrix(LocalMatrix);
noalias(LocalDisplacement) = prod(Matrix(trans(Rotation)), CurrentDisplacement);
CalculateBodyForce(Rotation, LocalBody, GlobalBody);
noalias(Stress) = -LocalBody + prod(LocalMatrix, LocalDisplacement);
// noalias(Stress) = -LocalBody + prod(Matrix(prod(Rotation,LocalMatrix)), LocalDisplacement);
return;
}
void dgSolver::CalculateForces()
{
m_firstPassCoef = dgFloat32(0.0f);
const dgInt32 passes = m_solverPasses;
const dgInt32 threadCounts = m_world->GetThreadCount();
for (dgInt32 step = 0; step < 4; step++) {
CalculateJointsAcceleration();
dgFloat32 accNorm = DG_SOLVER_MAX_ERROR * dgFloat32(2.0f);
for (dgInt32 k = 0; (k < passes) && (accNorm > DG_SOLVER_MAX_ERROR); k++) {
CalculateJointsForce();
CalculateBodyForce();
accNorm = dgFloat32(0.0f);
for (dgInt32 i = 0; i < threadCounts; i++) {
accNorm = dgMax(accNorm, m_accelNorm[i]);
}
}
IntegrateBodiesVelocity();
}
UpdateForceFeedback();
dgInt32 hasJointFeeback = 0;
for (dgInt32 i = 0; i < DG_MAX_THREADS_HIVE_COUNT; i++) {
hasJointFeeback |= m_hasJointFeeback[i];
}
CalculateBodiesAcceleration();
if (hasJointFeeback) {
UpdateKinematicFeedback();
}
}
//************************************************************************************
//************************************************************************************
void BeamElement::CalculateRHS(Vector& rRightHandSideVector)
{
Matrix Rotation;
Matrix GlobalMatrix;
Vector LocalBody;
array_1d<double, 12 > CurrentDisplacement;
Rotation.resize(12,12, false);
rRightHandSideVector = ZeroVector(12);
LocalBody = ZeroVector(12);
CalculateTransformationMatrix(Rotation);
CalculateBodyForce(Rotation, LocalBody, rRightHandSideVector);
CurrentDisplacement(0) = GetGeometry()[0].GetSolutionStepValue(DISPLACEMENT_X);
CurrentDisplacement(1) = GetGeometry()[0].GetSolutionStepValue(DISPLACEMENT_Y);
CurrentDisplacement(2) = GetGeometry()[0].GetSolutionStepValue(DISPLACEMENT_Z);
CurrentDisplacement(3) = GetGeometry()[0].GetSolutionStepValue(ROTATION_X);
CurrentDisplacement(4) = GetGeometry()[0].GetSolutionStepValue(ROTATION_Y);
CurrentDisplacement(5) = GetGeometry()[0].GetSolutionStepValue(ROTATION_Z);
CurrentDisplacement(6) = GetGeometry()[1].GetSolutionStepValue(DISPLACEMENT_X);
CurrentDisplacement(7) = GetGeometry()[1].GetSolutionStepValue(DISPLACEMENT_Y);
CurrentDisplacement(8) = GetGeometry()[1].GetSolutionStepValue(DISPLACEMENT_Z);
CurrentDisplacement(9) = GetGeometry()[1].GetSolutionStepValue(ROTATION_X);
CurrentDisplacement(10) = GetGeometry()[1].GetSolutionStepValue(ROTATION_Y);
CurrentDisplacement(11) = GetGeometry()[1].GetSolutionStepValue(ROTATION_Z);
CalculateLHS(GlobalMatrix);
noalias(rRightHandSideVector) -= prod(GlobalMatrix, CurrentDisplacement);
return;
}
