private void FixedUpdate()
{
foreach (Tuple <PotentialBase, RigidTripletType> pot_rigids_pair in m_PotentialRigidbodiesPairs)
{
PotentialBase potential = pot_rigids_pair.Item1;
RigidTripletType rigid_triplet = pot_rigids_pair.Item2;
Rigidbody rigid_i = rigid_triplet.Item1;
Rigidbody rigid_j = rigid_triplet.Item2;
Rigidbody rigid_k = rigid_triplet.Item3;
Vector3 r_ji = rigid_i.position - rigid_j.position;
Vector3 r_jk = rigid_k.position - rigid_j.position;
Vector3 e_ji = r_ji.normalized;
Vector3 e_jk = r_jk.normalized;
float cos_theta = Vector3.Dot(e_ji, e_jk);
float theta = Mathf.Acos(cos_theta);
float sin_theta = Mathf.Sin(theta);
float inv_sin_r_ji_len = 1.0f / (sin_theta * r_ji.magnitude);
float inv_sin_r_jk_len = 1.0f / (sin_theta * r_jk.magnitude);
float coef = potential.Derivative(theta);
Vector3 Fi = -coef * inv_sin_r_ji_len * (cos_theta * e_ji - e_jk);
Vector3 Fk = -coef * inv_sin_r_jk_len * (cos_theta * e_jk - e_ji);
rigid_i.AddForce(Fi);
rigid_k.AddForce(Fk);
rigid_j.AddForce(-(Fi + Fk));
}
}
private void FixedUpdate()
{
foreach (Tuple <PotentialBase, RigidPairType> pot_rigids_pair in m_PotentialRigidbodiesPairs)
{
PotentialBase potential = pot_rigids_pair.Item1;
RigidPairType rigid_pair = pot_rigids_pair.Item2;
Rigidbody rigid_i = rigid_pair.Item1;
Rigidbody rigid_j = rigid_pair.Item2;
Vector3 dist_vec = rigid_j.position - rigid_i.position;
Vector3 norm_vec = dist_vec.normalized;
float coef = potential.Derivative(dist_vec.magnitude);
rigid_i.AddForce(coef * norm_vec);
rigid_j.AddForce(-coef * norm_vec);
}
}
private void FixedUpdate()
{
foreach (Tuple <PotentialBase, RigidQuadrupletType> pot_rigids_pair in m_PotentialRigidbodiesPairs)
{
PotentialBase potential = pot_rigids_pair.Item1;
RigidQuadrupletType rigid_quadruplet = pot_rigids_pair.Item2;
Rigidbody rigid_i = rigid_quadruplet.Item1;
Rigidbody rigid_j = rigid_quadruplet.Item2;
Rigidbody rigid_k = rigid_quadruplet.Item3;
Rigidbody rigid_l = rigid_quadruplet.Item4;
Vector3 r_ji = rigid_i.position - rigid_j.position;
Vector3 r_jk = rigid_k.position - rigid_j.position;
Vector3 r_kj = -r_jk;
Vector3 r_lk = rigid_k.position - rigid_l.position;
Vector3 m = Vector3.Cross(r_ji, r_jk);
Vector3 n = Vector3.Cross(r_jk, r_lk);
float m_len = m.magnitude;
float n_len = n.magnitude;
float r_jk_len = r_jk.magnitude;
float r_jk_rlensq = 1.0f / (r_jk_len * r_jk_len);
float cos_phi = Mathf.Clamp(Vector3.Dot(m, n) / (m_len * n_len), -1.0f, 1.0f);
float phi = Mathf.Sign(Vector3.Dot(r_ji, n)) * Mathf.Acos(cos_phi);
float coef = potential.Derivative(phi);
Vector3 Fi = coef * r_jk_len / (m_len * m_len) * m;
Vector3 Fl = -coef * r_jk_len / (n_len * n_len) * n;
float coef_ijk = Vector3.Dot(r_ji, r_jk) * r_jk_rlensq;
float coef_jkl = Vector3.Dot(r_lk, r_jk) * r_jk_rlensq;
rigid_i.AddForce(Fi);
rigid_j.AddForce((coef_ijk - 1.0f) * Fi - coef_jkl * Fl);
rigid_k.AddForce((coef_jkl - 1.0f) * Fl - coef_ijk * Fi);
rigid_l.AddForce(Fl);
}
}