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