text: /*
   * Copyright 2020 Google LLC
   *
   * Licensed under the Apache License, Version 2.0 (the "License");
   * you may not use this file except in compliance with the License.
   * You may obtain a copy of the License at
   *
   *      http://www.apache.org/licenses/LICENSE-2.0
   *
   * Unless required by applicable law or agreed to in writing, software
   * distributed under the License is distributed on an "AS IS" BASIS,
   * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
   * See the License for the specific language governing permissions and
   * limitations under the License.
   */
  
  #ifndef TINY_CONSTRAINT_SOLVER_H
  #define TINY_CONSTRAINT_SOLVER_H
  
  #include "tiny_rigid_body.h"
  
  template <typename TinyScalar, typename TinyConstants>
  struct TinyContactPointRigidBody
      : public TinyContactPoint<TinyScalar, TinyConstants> {
    typedef ::TinyRigidBody<TinyScalar, TinyConstants> TinyRigidBody;
    TinyRigidBody* m_rigid_body_a;
    TinyRigidBody* m_rigid_body_b;
    TinyScalar m_restitution;
    TinyScalar m_friction;
  };
  
  template <typename TinyScalar, typename TinyConstants>
  struct TinyConstraintSolver {
    virtual ~TinyConstraintSolver() = default;
  
    // Note that the LCP(A, b) is not explicitly constructed.
    // Baumgarte stabilization is used to reduce positional drift.See description
    // in Michael Cline's thesis:
    // https ://www.cs.ubc.ca/grads/resources/thesis/Nov02/Michael_Cline.pdf
  
    // Args:
    // cp: contact point
    // dt : delta time (in seconds)
    virtual void resolveCollision(
        TinyContactPointRigidBody<TinyScalar, TinyConstants>& cp,
        TinyScalar dt) const {
      typedef ::TinyVector3<TinyScalar, TinyConstants> TinyVector3;
      TinyScalar erp =
          TinyConstants::fraction(1, 10);  // BAUMGARTE_ERROR_REDUCTION_PARAMETER
      const TinyVector3& world_point_a = cp.m_world_point_on_a;
      const TinyVector3& world_point_b = cp.m_world_point_on_b;
      TinyVector3 rel_pos_a =
          world_point_a - cp.m_rigid_body_a->m_world_pose.m_position;
      TinyVector3 rel_pos_b =
          world_point_b - cp.m_rigid_body_b->m_world_pose.m_position;
      TinyScalar baumgarte_rel_vel = TinyConstants::zero();
  
      if (cp.m_distance < TinyConstants::zero()) {
        baumgarte_rel_vel = erp * cp.m_distance / dt;
        TinyVector3 vel_a = cp.m_rigid_body_a->get_velocity(rel_pos_a);
        TinyVector3 vel_b = cp.m_rigid_body_b->get_velocity(rel_pos_b);
        TinyVector3 rel_vel = vel_a - vel_b;
        TinyScalar normal_rel_vel = cp.m_world_normal_on_b.dot(rel_vel);
        if (normal_rel_vel < TinyConstants::zero()) {
          TinyVector3 temp1 = cp.m_rigid_body_a->m_inv_inertia_world.dot(
              TinyVector3::cross2(rel_pos_a, cp.m_world_normal_on_b));
          TinyVector3 temp2 = cp.m_rigid_body_b->m_inv_inertia_world.dot(
              TinyVector3::cross2(rel_pos_b, cp.m_world_normal_on_b));
          TinyScalar ang =
              cp.m_world_normal_on_b.dot(TinyVector3::cross2(temp1, rel_pos_a) +
                                         TinyVector3::cross2(temp2, rel_pos_b));
          TinyScalar impulse =
              (-(TinyConstants::one() + cp.m_restitution) * normal_rel_vel -
               baumgarte_rel_vel) /
              (cp.m_rigid_body_a->m_inv_mass + cp.m_rigid_body_b->m_inv_mass +
               ang);
          if (impulse > TinyConstants::zero()) {
            TinyVector3 impulse_vector = impulse * cp.m_world_normal_on_b;
            cp.m_rigid_body_a->apply_impulse(impulse_vector, rel_pos_a);
            cp.m_rigid_body_b->apply_impulse(-impulse_vector, rel_pos_b);
  
            TinyVector3 lateral_rel_vel =
                rel_vel - normal_rel_vel * cp.m_world_normal_on_b;
            TinyScalar friction_impulse_trial =
                (lateral_rel_vel.length()) /
                (cp.m_rigid_body_a->m_inv_mass + cp.m_rigid_body_b->m_inv_mass +
                 ang);
  
            TinyScalar friction_coeffcient = cp.m_friction;
            TinyScalar friction_impulse;
            if (friction_impulse_trial < friction_coeffcient * impulse) {
              friction_impulse = friction_impulse_trial;
            } else {
              friction_impulse = friction_coeffcient * impulse;
            }
  
            if ((lateral_rel_vel.length()) > TinyConstants::fraction(1, 10000)) {
              TinyVector3 friction_dir =
                  lateral_rel_vel *
                  (TinyConstants::one() / lateral_rel_vel.length());
              cp.m_rigid_body_a->apply_impulse(-friction_impulse * friction_dir,
                                               rel_pos_a);
              cp.m_rigid_body_b->apply_impulse(friction_impulse * friction_dir,
                                               rel_pos_b);
            }
          }
        }
      }
    }
  };
  
  #endif  // TINY_CONSTRAINT_SOLVER_H
  