private Pendulum(Particle point_a_, Particle point_b_, Vector3f equilibrium_position_, Render callback_, params IConstraint[] constraints_)
{
point_a = point_a_;
point_b = point_b_;
callback = callback_;
equilibrium_position = equilibrium_position_;
constraints = new IConstraint[constraints_.Length];
for(int i = 0; i < constraints_.Length; ++i)
{
constraints[i] = constraints_[i];
}
}
public static Pendulum make(Particle point_a_, Particle point_b_, Vector3f equilibrium_position_, Render callback_, params IConstraint[] constraints_)
{
return new Pendulum(point_a_, point_b_, equilibrium_position_, callback_, constraints_);
}
private void update_point(Particle point_, Particle other_, float dt_)
{
point_.clear_force();
float theta = get_theta(point_.position, other_.position, equilibrium_position);
float m = point_.mass;
float g = -point_.gravity;
float v = (float)point_.velocity.magnitude;
float r = other_.distance(point_);
Vector3f gravity_direction = -new Vector3f(0f, g, 0f);
Vector3f tension_direction = (point_.position - other_.position);
gravity_direction.Normalize();
tension_direction.Normalize();
float Fg = m * g; // gravity force
float Fc = (float)((m * Math.Pow(v, 2)) / r); // centripetal force
float Ft = (float)(m * g * Math.Cos(theta)); // tension force
float T = Ft + Fc; // total tension force
point_.add_force(gravity_direction * Fg);
point_.add_force(tension_direction * T);
point_.update(dt_);
}
public float distance(Particle other_)
{
return (float)(position - other_.position).magnitude;
}
public static FixedDistanceConstraint make(Particle point_a_, Particle point_b_)
{
return new FixedDistanceConstraint(point_a_, point_b_);
}
private FixedDistanceConstraint(Particle point_a_, Particle point_b_)
{
_point_a = point_a_;
_point_b = point_b_;
_distance = _point_b.distance(_point_a);
}
