|
public static CreateIdentity ( int order ) : |
||
| order | int | |
| return | ||
public void GetMassInverse(MatrixXD massInv)
{
massInv.SetSubMatrix(index, index, massInv.SubMatrix(index, 3, index, 3)
+ 1.0f / Mass * DenseMatrixXD.CreateIdentity(3));
massInv.SetSubMatrix(index + 3, index + 3, massInv.SubMatrix(index + 3, 3, index + 3, 3)
+ m_inertiainv);
}
public void GetMass(MatrixXD mass)
{
mass.SetSubMatrix(index, index, mass.SubMatrix(index, 3, index, 3)
+ Mass * DenseMatrixXD.CreateIdentity(3));
mass.SetSubMatrix(index + 3, index + 3, mass.SubMatrix(index + 3, 3, index + 3, 3)
+ m_inertia);
}
public void GetForce(VectorXD force)
{
// TO BE COMPLETED
Vector3 posA = (bodyA != null) ? bodyA.PointLocalToGlobal(pointA) : pointA;
Vector3 posB = (bodyB != null) ? bodyB.PointLocalToGlobal(pointB) : pointB;
/////////////////////////////////////////////////////
///Calculo de C
float cx;
float cy;
float cz;
cx = (posA[0] - posB[0]);
cy = (posA[1] - posB[1]);
cz = (posA[2] - posB[2]);
VectorXD c = Utils.ToVectorXD(new Vector3(cx, cy, cz)); //|xa - xb|
/////////////////////////////////////////////////////
/////////////////////////////////////////////////////
///Calculo de GradC y F
MatrixXD identity = DenseMatrixXD.CreateIdentity(3);
MatrixXD dcdax = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdaTheta = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdbx = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdbTheta = MatrixXD.Build.Dense(3, 3);
if (bodyA != null)
{
dcdax = identity;
dcdaTheta = -Utils.Skew(posA - bodyA.m_pos);
VectorXD fax = -Stiffness *dcdax.Transpose() * c;
VectorXD faTheta = -Stiffness *dcdaTheta.Transpose() * c;
force.SetSubVector(bodyA.index, 3, force.SubVector(bodyA.index, 3) + fax);
force.SetSubVector(bodyA.index + 3, 3, force.SubVector(bodyA.index + 3, 3) + faTheta);
}
if (bodyB != null)
{
dcdbx = -identity;
dcdbTheta = Utils.Skew(posB - bodyB.m_pos);
VectorXD fbx = -Stiffness *dcdbx.Transpose() * c;
VectorXD fbTheta = -Stiffness *dcdbTheta.Transpose() * c;
force.SetSubVector(bodyB.index, 3, force.SubVector(bodyB.index, 3) + fbx);
force.SetSubVector(bodyB.index + 3, 3, force.SubVector(bodyB.index + 3, 3) + fbTheta);
}
/////////////////////////////////////////////////////
}
public void GetForceJacobian(MatrixXD dFdx, MatrixXD dFdv)
{
// TO BE COMPLETED
//Derivative of damping on the linear force
dFdv.SetSubMatrix(index, index, dFdv.SubMatrix(index, 3, index, 3)
- Damping * Mass * DenseMatrixXD.CreateIdentity(3));
//Derivative of damping on the torque and of the quadratic velocity vector
// T = skew(M * w) * w = - skew(w) * (M * w)
// dTdw = skew(M * w) - skew(w) * M
dFdv.SetSubMatrix(index + 3, index + 3, dFdv.SubMatrix(index + 3, 3, index + 3, 3) - Damping * m_inertia
+ Utils.Skew(Utils.ToVector3(m_inertia * Utils.ToVectorXD(m_omega))) - Utils.Skew(m_omega) * m_inertia);
}
/// <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);
}
}
// Compute the velocity and angular velocity derivatives for a body, if exists
private void getConstraintJacobianForBody(RigidBody body, Vector3 localPoint, MatrixXD J, float sign)
{
if (body)
{
int vi = body.getSimIndex();
int wi = vi + 3;
MatrixXD Jv = DenseMatrixXD.CreateIdentity(3);
MatrixXD Jw = Utils.Skew(-body.VecLocalToGlobal(localPoint));
J.SetSubMatrix(m_index, 3, vi, 3, (Jv * sign));
J.SetSubMatrix(m_index, 3, wi, 3, (Jw * sign));
}
}
public void GetForce(VectorXD force)
{
// TO BE COMPLETED
Vector3 posA = (bodyA != null) ? bodyA.PointLocalToGlobal(pointA) : pointA;
Vector3 posB = (bodyB != null) ? bodyB.PointLocalToGlobal(pointB) : pointB;
float cx, cy, cz;
cx = (posA[0] - posB[0]);
cy = (posA[1] - posB[1]);
cz = (posA[2] - posB[2]);
MatrixXD dcdPosa = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdPosb = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdQa = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdQb = MatrixXD.Build.Dense(3, 3);
MatrixXD I = DenseMatrixXD.CreateIdentity(3);
if (bodyA != null)
{
dcdPosa = I;
dcdQa = -Utils.Skew(posA - bodyA.m_pos);
VectorXD faPos = -Stiffness *dcdPosa.Transpose() * Utils.ToVectorXD(new Vector3(cx, cy, cz));
VectorXD faQ = -Stiffness *dcdQa.Transpose() * Utils.ToVectorXD(new Vector3(cx, cy, cz));
force.SetSubVector(bodyA.index, 3, force.SubVector(bodyA.index, 3) + faPos);
force.SetSubVector(bodyA.index + 3, 3, force.SubVector(bodyA.index + 3, 3) + faQ);
}
if (bodyB != null)
{
dcdPosb = -I;
dcdQb = Utils.Skew(posB - bodyB.m_pos);
VectorXD fbPos = -Stiffness *dcdPosb.Transpose() * Utils.ToVectorXD(new Vector3(cx, cy, cz));
VectorXD fbQ = -Stiffness *dcdQb.Transpose() * Utils.ToVectorXD(new Vector3(cx, cy, cz));
force.SetSubVector(bodyB.index, 3, force.SubVector(bodyB.index, 3) + fbPos);
force.SetSubVector(bodyB.index + 3, 3, force.SubVector(bodyB.index + 3, 3) + fbQ);
}
}
/// <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];
}
}
}
public void Initialize(int ind, PhysicsManager m)
{
Manager = m;
index = ind;
// Initialize inertia. We assume that the object is connected to a Cube mesh.
Transform xform = GetComponent <Transform>();
if (xform == null)
{
System.Console.WriteLine("[ERROR] Couldn't find any transform to the rigid body");
}
else
{
System.Console.WriteLine("[TRACE] Succesfully found transform connected to the rigid body");
}
if (xform != null)
{
m_inertia0 = DenseMatrixXD.CreateIdentity(3);
double[] vals;
vals = new double[3];
vals[0] = 1.0f / 12.0f * Mass * (xform.localScale.y * xform.localScale.y + xform.localScale.z * xform.localScale.z);
vals[1] = 1.0f / 12.0f * Mass * (xform.localScale.x * xform.localScale.x + xform.localScale.z * xform.localScale.z);
vals[2] = 1.0f / 12.0f * Mass * (xform.localScale.x * xform.localScale.x + xform.localScale.y * xform.localScale.y);
m_inertia0.SetDiagonal(vals);
}
// Initialize kinematics
m_pos = xform.position;
m_rot = xform.rotation;
m_vel = Vector3.zero;
m_omega = Vector3.zero;
// Initialize all inertia terms
m_inertia0inv = m_inertia0.Inverse();
m_inertia = Utils.WarpMatrix(m_rot, m_inertia0);
m_inertiainv = Utils.WarpMatrix(m_rot, m_inertia0inv);
}
public void GetConstraintJacobian(MatrixXD dcdx) //Gradiente de C
{
// TO BE COMPLETED
Vector3 posA = (bodyA != null) ? bodyA.PointLocalToGlobal(pointA) : pointA;
Vector3 posB = (bodyB != null) ? bodyB.PointLocalToGlobal(pointB) : pointB;
MatrixXD dcdPosa = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdPosb = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdQa = MatrixXD.Build.Dense(3, 3);
MatrixXD dcdQb = MatrixXD.Build.Dense(3, 3);
MatrixXD I = DenseMatrixXD.CreateIdentity(3);
float cx, cy, cz;
cx = (posA[0] - posB[0]);
cy = (posA[1] - posB[1]);
cz = (posA[2] - posB[2]);
if (bodyA != null)
{
dcdPosa = I;
dcdQa = -Utils.Skew(posA - bodyA.m_pos);
for (int i = 0; i < 3; i++)
{
dcdx.SetSubMatrix(index, bodyA.index, dcdx.SubMatrix(index, 3, bodyA.index, 3) + dcdPosa);
dcdx.SetSubMatrix(index, bodyA.index + 3, dcdx.SubMatrix(index, 3, bodyA.index + 3, 3) + dcdQa);
}
}
if (bodyB != null)
{
dcdPosb = -I;
dcdQb = Utils.Skew(posB - bodyB.m_pos);
for (int i = 0; i < 3; i++)
{
dcdx.SetSubMatrix(index, bodyB.index, dcdx.SubMatrix(index, 3, bodyB.index, 3) + dcdPosb);
dcdx.SetSubMatrix(index, bodyB.index + 3, dcdx.SubMatrix(index, 3, bodyB.index + 3, 3) + dcdQb);
}
}
//Esto es gracias a mi explicación super a sucio que voy a pasar a limpio a que sí Nerea? :D
}
// Nothing to do here
#endregion
#region ISimulable
public void initialize(int index, PhysicsManager manager)
{
m_manager = manager;
// Initialize indices
m_index = index;
// Initialize inertia. We assume that the object is connected to a Cube mesh.
Transform xform = this.GetComponent <Transform>();
if (xform == null)
{
System.Console.WriteLine("[ERROR] Couldn't find any transform connected to the rigid body");
}
else
{
System.Console.WriteLine("[TRACE] Succesfully found transform connected to the rigid body");
}
if (xform != null)
{
this.m_inertia0 = DenseMatrixXD.CreateIdentity(3);
double[] vals;
vals = new double[3];
vals[0] = 1.0f / 12.0f * mass * (xform.localScale.y * xform.localScale.y + xform.localScale.z * xform.localScale.z);
vals[1] = 1.0f / 12.0f * mass * (xform.localScale.x * xform.localScale.x + xform.localScale.z * xform.localScale.z);
vals[2] = 1.0f / 12.0f * mass * (xform.localScale.x * xform.localScale.x + xform.localScale.y * xform.localScale.y);
this.m_inertia0.SetDiagonal(vals);
this.m_inertia = m_inertia0;
}
// Initialize kinematics
this.m_pos = xform.position;
this.m_rot = xform.rotation;
this.m_vel = Vector3.zero;
this.m_omega = Vector3.zero;
}
public void getConstraintJacobian(MatrixXD J)
{
// TO BE COMPLETED: WRITE CONSTRAINT JACOBIANS TO J
// Apuntes del día 5 :
// La jacobiana es un conjunto de V y W formada como
// V,W,V,W... siendo (V,W) un rigidbody
// Wa y Wb son matrices 3X3 formadas por (-Ra * ra) * <- Skewed
// Va y Vb son matrices identidad 3x3
// Metemos (si hay cuerpo) -> Identidad y después Wa/Wb
// un truco es meter la identidad en B como negativa y así ya es opuesto a Va
// Va = -Vb
MatrixXD IdentityA;
MatrixXD IdentityB;
MatrixXD Wa, Wb;
if (BodyA != null)
{
Wa = Utils.Skew(-(BodyA.Rotation * m_pA));
IdentityA = DenseMatrixXD.CreateIdentity(3);
J.SetSubMatrix(m_index, BodyA.getSimIndex(), IdentityA);
J.SetSubMatrix(m_index, BodyA.getSimIndex() + 3, Wa);
}
if (BodyB != null)
{
Wb = Utils.Skew((BodyB.Rotation * m_pB));
IdentityB = -DenseMatrixXD.CreateIdentity(3);
J.SetSubMatrix(m_index, BodyB.getSimIndex(), IdentityB);
J.SetSubMatrix(m_index, BodyB.getSimIndex() + 3, Wb);
}
}
public void getMassMatrix(MatrixXD mmout)
{
mmout.SetSubMatrix(m_index, m_index, this.mass * DenseMatrixXD.CreateIdentity(3));
mmout.SetSubMatrix(m_index + 3, m_index + 3, this.m_inertia);
}
private void SoftConstraintsStep()
{
// Apuntes Miki + apuntes Marta + apuntes clase
// Step de Débiles:
// Podemos integrar cada rigidbody por separado o tener un solo sistema V siendo un vector V,W
// Teniendo M * v' = F
// Siendo M:
// | m*Identidad 0 |
// | 0 MInercia |
// y F = | F |
// | T |
// A * v = b
// M * V = M*V0 + timestep * F
// b = M*V0 + timestep * F
// La matriz de masas es numero de objetos * 6
MatrixXD massMatrix = DenseMatrixXD.CreateIdentity(m_objs.Count * 6);
VectorXD total_force = new DenseVectorXD(m_objs.Count * 6);
VectorXD total_velocities = new DenseVectorXD(m_objs.Count * 6);
// Paso 0: Limpiamos las fuerzas y matrices anteriores para evitar sumas de error - Podríamos meterlo después, but
foreach (RigidBody obj in m_objs)
{
obj.clearForcesAndMatrices();
}
// Paso 1: añadimos las fuerzas del constraint
// En ellas hacemos Fa y Fb (Si tienen los cuerpos)
// Y añadimos el torque a los rigidbodies
foreach (Constraint c in m_constraints)
{
c.addForces();
}
// Paso 2: Establecemos la parte del objeto en la matriz de masas (Pondremos la inercia como inercia 0 porque si no es null->RigidBody.cs
// Paso 3: Metemos las velocidades y las fuerzas (y rotaciones)
// Fuertes deberían ser practicamente iguales pero añadiendo las jacobianas
foreach (RigidBody obj in m_objs)
{
obj.getMassMatrix(massMatrix);
obj.getVelocityVector(total_velocities);
obj.addForcesAndMatrices();
obj.getForceVector(total_force);
}
// Resolvemos el sistema
VectorXD b = (massMatrix * total_velocities) + (TimeStep * total_force);
VectorXD newVelocities = massMatrix.Solve(b);
// Y modificamos posiciones
foreach (RigidBody obj in m_objs)
{
obj.setVelocityVector(newVelocities);
obj.advancePosition();
}
}
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();
}
}
