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