public static Create ( int rows, int columns, Func |
||
rows | int | |
columns | int | |
init | Func |
|
return |
private void HardConstraintsStep() { // Vectors and matrices to store data VectorXD C0 = new DenseVectorXD(m_numcs); VectorXD f = new DenseVectorXD(m_numdofs); VectorXD v = new DenseVectorXD(m_numdofs); MatrixXD M = new DenseMatrixXD(m_numdofs, m_numdofs); MatrixXD J = new DenseMatrixXD(m_numcs, m_numdofs); MatrixXD System = new DenseMatrixXD(m_numcs + m_numdofs, m_numcs + m_numdofs); VectorXD Independent = new DenseVectorXD(m_numdofs + m_numcs); // Compute forces and retrieve the rigid bodies data foreach (ISimulable i in m_objs) { i.clearForcesAndMatrices(); i.addForcesAndMatrices(); i.getForceVector(f); i.getVelocityVector(v); i.getMassMatrix(M); } // Compute and retrieve restrictions and Jacobians foreach (Constraint c in m_constraints) { c.getConstraintVector(C0); c.getConstraintJacobian(J); } // Set up left-side system System.SetSubMatrix(0, m_numdofs, 0, m_numdofs, M); System.SetSubMatrix(m_numdofs, m_numcs, 0, m_numdofs, J); System.SetSubMatrix(0, m_numdofs, m_numdofs, m_numcs, J.Transpose()); System.SetSubMatrix(m_numdofs, m_numcs, m_numdofs, m_numcs, DenseMatrixXD.Create(m_numcs, m_numcs, 0.0)); // Set up right-side system VectorXD b = M * v + TimeStep * f; VectorXD AtC0 = (-1.0f / TimeStep) * C0; Independent.SetSubVector(0, m_numdofs, b); Independent.SetSubVector(m_numdofs, m_numcs, AtC0); // Solve system VectorXD newVelocities = System.Solve(Independent); // Update bodies foreach (ISimulable i in m_objs) { i.setVelocityVector(newVelocities); i.advancePosition(); } }
/// <summary> /// Adds the elastic Jacobian corresponding to the string /// to the specified global Jacobian matrix at the index /// associated with edge nodes. Note |mJSum| = (nDOF,nDOF). /// </summary> public void addJacobian(MatrixXD mJsum) { Node a = Edge.Node1; Node b = Edge.Node0; int index1 = a.SimIdx * 3; int index0 = b.SimIdx * 3; Vector3 LR3 = a.Xt - b.Xt; float L = LR3.magnitude; float kLL0 = Ke * (L - L0); Vector3 U = LR3.normalized; MatrixXD I = DenseMatrixXD.CreateIdentity(3); MatrixXD uut = DenseMatrixXD.Create(3, 3, 0.0); for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { uut [i, j] = U [i] * U [j]; } } MatrixXD dUdxa = 1.0f / L * (I - uut); MatrixXD dFadxa = kLL0 * dUdxa + Ke * uut; for (int i = 0; i < 3; i++) { for (int j = 0; j < 3; j++) { mJsum [index1 + i, index1 + j] += dFadxa[i, j]; mJsum [index0 + i, index0 + j] += dFadxa[i, j]; mJsum [index1 + i, index0 + j] += -dFadxa[i, j]; mJsum [index0 + i, index1 + j] += -dFadxa[i, j]; } } }