double
FEI3dWedgeLin :: giveTransformationJacobian(const FloatArray &lcoords, const FEICellGeometry &cellgeo)
{
FloatMatrix jacobianMatrix(3, 3);
this->giveJacobianMatrixAt(jacobianMatrix, lcoords, cellgeo);
return jacobianMatrix.giveDeterminant() / 2.; ///@todo Should this really be a factor 1/2 here?
}
double
FEI3dHexaQuad :: giveTransformationJacobian(const FloatArray &lcoords, const FEICellGeometry &cellgeo)
{
FloatMatrix jacobianMatrix(3, 3);
this->giveJacobianMatrixAt(jacobianMatrix, lcoords, cellgeo);
return jacobianMatrix.giveDeterminant();
}
dgJacobian dgDynamicBody::IntegrateForceAndToque(const dgVector& force, const dgVector& torque, const dgVector& timestep)
{
dgJacobian velocStep;
if (m_gyroTorqueOn) {
dgVector dtHalf(timestep * dgVector::m_half);
dgMatrix matrix(m_gyroRotation, dgVector::m_wOne);
dgVector localOmega(matrix.UnrotateVector(m_omega));
dgVector localTorque(matrix.UnrotateVector(torque - m_gyroTorque));
// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
dgVector dw(localOmega * dtHalf);
dgMatrix jacobianMatrix(
dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)),
dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(1.0f)),
dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(1.0f)),
dgVector::m_wOne);
// and solving for alpha we get the angular acceleration at t + dt
// calculate gradient at a full time step
//dgVector gradientStep(localTorque * timestep);
dgVector gradientStep(jacobianMatrix.SolveByGaussianElimination(localTorque * timestep));
dgVector omega(matrix.RotateVector(localOmega + gradientStep));
dgAssert(omega.m_w == dgFloat32(0.0f));
// integrate rotation here
dgFloat32 omegaMag2 = omega.DotProduct(omega).GetScalar() + dgFloat32(1.0e-12f);
dgFloat32 invOmegaMag = dgRsqrt(omegaMag2);
dgVector omegaAxis(omega.Scale(invOmegaMag));
dgFloat32 omegaAngle = invOmegaMag * omegaMag2 * timestep.GetScalar();
dgQuaternion deltaRotation(omegaAxis, omegaAngle);
m_gyroRotation = m_gyroRotation * deltaRotation;
dgAssert((m_gyroRotation.DotProduct(m_gyroRotation) - dgFloat32(1.0f)) < dgFloat32(1.0e-5f));
matrix = dgMatrix(m_gyroRotation, dgVector::m_wOne);
localOmega = matrix.UnrotateVector(omega);
//dgVector angularMomentum(inertia * localOmega);
//body->m_gyroTorque = matrix.RotateVector(localOmega.CrossProduct(angularMomentum));
//body->m_gyroAlpha = body->m_invWorldInertiaMatrix.RotateVector(body->m_gyroTorque);
dgVector localGyroTorque(localOmega.CrossProduct(m_mass * localOmega));
m_gyroTorque = matrix.RotateVector(localGyroTorque);
m_gyroAlpha = matrix.RotateVector(localGyroTorque * m_invMass);
velocStep.m_angular = matrix.RotateVector(gradientStep);
} else {
velocStep.m_angular = m_invWorldInertiaMatrix.RotateVector(torque) * timestep;
//velocStep.m_angular = velocStep.m_angular * dgVector::m_half;
}
velocStep.m_linear = force.Scale(m_invMass.m_w) * timestep;
return velocStep;
}
NOX::Abstract::Group::ReturnType
LOCA::LAPACK::Group::augmentJacobianForHomotopy(double a, double b)
{
NOX::LAPACK::Matrix<double>& jacobianMatrix = jacSolver.getMatrix();
int size = jacobianMatrix.numRows();
// Scale the matrix by the value of the homotopy continuation param
jacobianMatrix.scale(a);
// Add the scaled identity matrix to the jacobian
for (int i = 0; i < size; i++)
jacobianMatrix(i,i) += b;
return NOX::Abstract::Group::Ok;
}
double
FEI3dWedgeLin :: evaldNdx(FloatMatrix &answer, const FloatArray &lcoords, const FEICellGeometry &cellgeo)
{
FloatMatrix jacobianMatrix(2, 2), inv, dNduvw, coords;
this->giveLocalDerivative(dNduvw, lcoords);
coords.resize(3, 6);
for ( int i = 1; i <= 6; i++ ) {
coords.setColumn(* cellgeo.giveVertexCoordinates(i), i);
}
jacobianMatrix.beProductOf(coords, dNduvw);
inv.beInverseOf(jacobianMatrix);
answer.beProductOf(dNduvw, inv);
return jacobianMatrix.giveDeterminant();
}
NOX::Abstract::Group::ReturnType
LOCA::LAPACK::Group::computeComplex(double frequency)
{
string callingFunction = "LOCA::LAPACK::computeComplex()";
#ifdef HAVE_TEUCHOS_COMPLEX
NOX::Abstract::Group::ReturnType finalStatus;
freq = frequency;
// Compute Jacobian
finalStatus = computeJacobian();
globalData->locaErrorCheck->checkReturnType(finalStatus, callingFunction);
// Compute Mass matrix
bool res =
locaProblemInterface.computeShiftedMatrix(0.0, 1.0, xVector,
shiftedSolver.getMatrix());
// Compute complex matrix
NOX::LAPACK::Matrix<double>& jacobianMatrix = jacSolver.getMatrix();
NOX::LAPACK::Matrix<double>& massMatrix = shiftedSolver.getMatrix();
NOX::LAPACK::Matrix< std::complex<double> >& complexMatrix =
complexSolver.getMatrix();
int n = jacobianMatrix.numRows();
for (int j=0; j<n; j++) {
for (int i=0; i<n; i++) {
complexMatrix(i,j) =
std::complex<double>(jacobianMatrix(i,j), frequency*massMatrix(i,j));
}
}
if (finalStatus == NOX::Abstract::Group::Ok && res)
isValidComplex = true;
if (res)
return finalStatus;
else
return NOX::Abstract::Group::Failed;
#else
globalData->locaErrorCheck->throwError(
callingFunction,
"TEUCHOS_COMPLEX must be enabled for complex support! Reconfigure with -D Teuchos_ENABLE_COMPLEX");
return NOX::Abstract::Group::BadDependency;
#endif
}
double
FEI2dQuadLin :: evaldNdx(FloatMatrix &answer, const FloatArray &lcoords, const FEICellGeometry &cellgeo)
{
FloatMatrix jacobianMatrix(2, 2), inv, dn;
this->giveDerivatives(dn, lcoords);
for ( int i = 1; i <= dn.giveNumberOfRows(); i++ ) {
double x = cellgeo.giveVertexCoordinates(i)->at(xind);
double y = cellgeo.giveVertexCoordinates(i)->at(yind);
jacobianMatrix.at(1, 1) += dn.at(i, 1) * x;
jacobianMatrix.at(1, 2) += dn.at(i, 1) * y;
jacobianMatrix.at(2, 1) += dn.at(i, 2) * x;
jacobianMatrix.at(2, 2) += dn.at(i, 2) * y;
}
inv.beInverseOf(jacobianMatrix);
answer.beProductTOf(dn, inv);
return jacobianMatrix.giveDeterminant();
}
void dgDynamicBody::IntegrateImplicit(dgFloat32 timestep)
{
// using simple backward Euler or implicit integration, this is.
// f'(t + dt) = (f(t + dt) - f(t)) / dt
// therefore:
// f(t + dt) = f(t) + f'(t + dt) * dt
// approximate f'(t + dt) by expanding the Taylor of f(w + dt)
// f(w + dt) = f(w) + f'(w) * dt + f''(w) * dt^2 / 2! + ....
// assume dt^2 is negligible, therefore we can truncate the expansion to
// f(w + dt) ~= f(w) + f'(w) * dt
// calculating dw as the f'(w) = d(wx, wy, wz) | dt
// dw/dt = a = (Tl - (wl x (wl * Il)) * Il^1
// expanding f(w)
// f'(wx) = Ix * ax = Tx - (Iz - Iy) * wy * wz
// f'(wy) = Iy * ay = Ty - (Ix - Iz) * wz * wx
// f'(wz) = Iz * az = Tz - (Iy - Ix) * wx * wy
//
// calculation the expansion
// Ix * ax = (Tx - (Iz - Iy) * wy * wz) - ((Iz - Iy) * wy * az + (Iz - Iy) * ay * wz) * dt
// Iy * ay = (Ty - (Ix - Iz) * wz * wx) - ((Ix - Iz) * wz * ax + (Ix - Iz) * az * wx) * dt
// Iz * az = (Tz - (Iy - Ix) * wx * wy) - ((Iy - Ix) * wx * ay + (Iy - Ix) * ax * wy) * dt
//
// factorizing a we get
// Ix * ax + (Iz - Iy) * dwy * az + (Iz - Iy) * dwz * ay = Tx - (Iz - Iy) * wy * wz
// Iy * ay + (Ix - Iz) * dwz * ax + (Ix - Iz) * dwx * az = Ty - (Ix - Iz) * wz * wx
// Iz * az + (Iy - Ix) * dwx * ay + (Iy - Ix) * dwy * ax = Tz - (Iy - Ix) * wx * wy
dgVector localOmega(m_matrix.UnrotateVector(m_omega));
dgVector localTorque(m_matrix.UnrotateVector(m_externalTorque - m_gyroTorque));
// and solving for alpha we get the angular acceleration at t + dt
// calculate gradient at a full time step
dgVector gradientStep(localTorque.Scale(timestep));
// derivative at half time step. (similar to midpoint Euler so that it does not loses too much energy)
dgVector dw(localOmega.Scale(dgFloat32(0.5f) * timestep));
//dgVector dw(localOmega.Scale(dgFloat32(1.0f) * timestep));
// calculates Jacobian matrix (
// dWx / dwx, dWx / dwy, dWx / dwz
// dWy / dwx, dWy / dwy, dWy / dwz
// dWz / dwx, dWz / dwy, dWz / dwz
//
// dWx / dwx = Ix, dWx / dwy = (Iz - Iy) * wz * dt, dWx / dwz = (Iz - Iy) * wy * dt)
// dWy / dwx = (Ix - Iz) * wz * dt, dWy / dwy = Iy, dWy / dwz = (Ix - Iz) * wx * dt
// dWz / dwx = (Iy - Ix) * wy * dt, dWz / dwy = (Iy - Ix) * wx * dt, dWz / dwz = Iz
dgMatrix jacobianMatrix(
dgVector(m_mass[0], (m_mass[2] - m_mass[1]) * dw[2], (m_mass[2] - m_mass[1]) * dw[1], dgFloat32(0.0f)),
dgVector((m_mass[0] - m_mass[2]) * dw[2], m_mass[1], (m_mass[0] - m_mass[2]) * dw[0], dgFloat32(0.0f)),
dgVector((m_mass[1] - m_mass[0]) * dw[1], (m_mass[1] - m_mass[0]) * dw[0], m_mass[2], dgFloat32(0.0f)),
dgVector::m_wOne);
gradientStep = jacobianMatrix.SolveByGaussianElimination(gradientStep);
localOmega += gradientStep;
m_accel = m_externalForce.Scale(m_invMass.m_w);
m_alpha = m_matrix.RotateVector(localTorque * m_invMass);
m_veloc += m_accel.Scale(timestep);
m_omega = m_matrix.RotateVector(localOmega);
}
