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> /// Performs a simulation step using Symplectic integration with constrained dynamics. /// The constraints are treated as implicit /// </summary> private void stepSymplecticConstraints() { // TO BE COMPLETED VectorXD v = new DenseVectorXD(m_numDoFs); VectorXD f = new DenseVectorXD(m_numDoFs); MatrixXD Minv = new DenseMatrixXD(m_numDoFs); MatrixXD J = new DenseMatrixXD(m_numConstraints, m_numDoFs); MatrixXD A = new DenseMatrixXD(m_numDoFs); VectorXD B = new DenseVectorXD(m_numDoFs); VectorXD c = new DenseVectorXD(m_numConstraints); VectorXD b = new DenseVectorXD(m_numDoFs); VectorXD lamda = new DenseVectorXD(m_numConstraints); MatrixXD I = DenseMatrixXD.CreateIdentity(m_numDoFs); f.Clear(); Minv.Clear(); J.Clear(); foreach (ISimulable obj in m_objs) { obj.GetVelocity(v); obj.GetForce(f); obj.GetMassInverse(Minv); } foreach (IConstraint constraint in m_constraints) { constraint.GetForce(f); constraint.GetConstraints(c); constraint.GetConstraintJacobian(J); } foreach (ISimulable obj in m_objs) { obj.FixVector(f); obj.FixMatrix(Minv); } b = v + TimeStep * Minv * f; A = J * I * J.Transpose(); B = J * I * b + (1.0f / TimeStep) * c; lamda = A.Solve(B); v = I * (b - J.Transpose() * lamda); VectorXD x = TimeStep * v; foreach (ISimulable obj in m_objs) { obj.AdvanceIncrementalPosition(x); obj.SetVelocity(v); } }
/// <summary> /// Performs a simulation step using Implicit integration. /// </summary> private void stepImplicit() { // TO BE COMPLETED VectorXD x = new DenseVectorXD(m_numDoFs); VectorXD v = new DenseVectorXD(m_numDoFs); VectorXD f = new DenseVectorXD(m_numDoFs); MatrixXD M = new DenseMatrixXD(m_numDoFs); MatrixXD Jk = new DenseMatrixXD(m_numDoFs); MatrixXD Jd = new DenseMatrixXD(m_numDoFs); MatrixXD A = new DenseMatrixXD(m_numDoFs); VectorXD B = new DenseVectorXD(m_numDoFs); M.Clear(); foreach (ISimulable obj in m_objs) { obj.GetPosition(x); obj.GetVelocity(v); obj.ComputeForces(); obj.GetForce(f); obj.GetMass(M); obj.GetForceJacobian(Jk, Jd); } foreach (ISimulable obj in m_objs) { obj.FixVector(f); obj.FixMatrix(M); } A = M + TimeStep * Jd + TimeStep * TimeStep * -Jk; B = (M + TimeStep * Jd) * v + TimeStep * f; v = A.Solve(B); x += TimeStep * v; foreach (ISimulable obj in m_objs) { obj.FixVector(v); obj.SetPosition(x); obj.SetVelocity(v); } }
private void HardConstraintsStep() { // Apuntes miki + apuntes clase // Step de Fuertes: // Tenemos que conseguir una matriz enorme formada por // // | A J^t | // | J 0 | // A es la matriz de masas-> 6 * cada componente como en soft // J es la matriz jacobiana que rellenamos en Constraint.getConstraintJacobian // J se forma 3 * numero de constraints, 6* num rigidbodies // J^t se forma 6* num rigidbodies, 3 * numero de constraints // Recordemos que cada rigidbody tiene 6 numdof // C0 = 3 * numero de constraints<- Es similar a C en soft MatrixXD massMatrix = DenseMatrixXD.CreateIdentity(m_objs.Count * 6); MatrixXD jacobian = new DenseMatrixXD(3 * Constraints.Count, 6 * m_objs.Count); VectorXD C0 = new DenseVectorXD(3 * m_constraints.Count); // paso 1: necesitamos settear la jacobiana y C0 para cada restricción foreach (Constraint c in m_constraints) { c.getConstraintVector(C0); c.getConstraintJacobian(jacobian); } // las fuerzas son 1 x 6 * m_objs.Count, igual que las velocidades. // Parece que se mete el torque y demás. 3 * objetos no funciona VectorXD total_force = new DenseVectorXD(6 * m_objs.Count); VectorXD total_velocities = new DenseVectorXD(6 * m_objs.Count); // PARA CADA RIGIDBODY // Paso 2: Limpiamos las fuerzas y matrices anteriores para evitar sumas de error // Paso 3: Establecemos la matriz de masas (Pondremos la inercia como inercia 0 porque si no es null->RigidBody.cs // Paso 4: Metemos valores en las velocidades anteriores, ¿Por qué? Porque Al final haremos un A * v = b // como en las prácticas anteriores y b es un vector formado por b y -1 * C0 / TimeStep // siendo b la fórmula (matriz_de_masas * velocidades) + (TimeStep * fuerzas); // Paso 5: Añadimos fuerzas y matrices (que las hemos limpiado antes) // Paso 6: Metemos las nuevas fuerzas (getForceVector) en el vector de fuerzas foreach (RigidBody obj in m_objs) { obj.clearForcesAndMatrices(); obj.getMassMatrix(massMatrix); obj.getVelocityVector(total_velocities); obj.addForcesAndMatrices(); obj.getForceVector(total_force); } // Creamos la super matriz que hemos dicho con la de masas, jacobianas y ceros // Al crear una MtrixXD se crea con ceros. MatrixXD jacobianT = jacobian.Transpose(); MatrixXD ceroMatrix = new DenseMatrixXD(3 * m_constraints.Count, 3 * m_constraints.Count); DenseMatrixXD megaMatrix = new DenseMatrixXD(jacobian.RowCount + massMatrix.RowCount, jacobianT.ColumnCount + massMatrix.ColumnCount); // la matriz de masas es 0,0 a (m_objs.Count * 6) // Una vez acaba esa, va la jacobiana traspuesta // Debajo de la misma manera va la jacobiana // y en la esquina abajo derecha va la de ceros megaMatrix.SetSubMatrix(0, 0, massMatrix); megaMatrix.SetSubMatrix(0, 0 + massMatrix.ColumnCount, jacobianT); megaMatrix.SetSubMatrix(0 + massMatrix.RowCount, 0, jacobian); megaMatrix.SetSubMatrix(0 + massMatrix.RowCount, 0 + massMatrix.ColumnCount, ceroMatrix); // M * v = M * v0 + timestep * fuerzas // V0 es la velocidad del step anterior // en A * v = b, b lo dividiremos como b y b2 // y los concatenamos VectorXD b = (massMatrix * total_velocities) + (TimeStep * total_force); VectorXD b2 = -1 * C0 / TimeStep; // b total - ambos bs VectorXD realB = new DenseVectorXD(b.Count + b2.Count); realB.SetSubVector(0, b.Count, b); realB.SetSubVector(b.Count, b2.Count, b2); // V se forma por velocidades y un vector de lamdas, que debe ser del tamaño de C0 ya que // b2 es escalar * c0 VectorXD lamdas = new DenseVectorXD(C0.Count); // conjunto de velocidades formada por las velocidades y las lamdas VectorXD megaV = new DenseVectorXD(total_velocities.Count + lamdas.Count); megaV.SetSubVector(0, total_velocities.Count, total_velocities); megaV.SetSubVector(total_velocities.Count, lamdas.Count, lamdas); // Resolvemos el sistema megaV = megaMatrix.Solve(realB); // nueva velocidad VectorXD newVelocities = megaV.SubVector(0, total_velocities.Count); // Establecemos las nuevas posiciones y velocidades foreach (RigidBody obj in m_objs) { obj.setVelocityVector(newVelocities); obj.advancePosition(); } }