Initial commit.

seesaw.cpp

@@ -0,0 +1,117 @@
+// -*- C++ -*-
+
+#include <OpenGL/gl.h>
+
+#include "material.h"
+#include "physics/constraint.h"
+#include "physics/force.h"
+#include "physics/particle.h"
+#include "physics/shape.h"
+#include "physics/vector.h"
+#include "seesaw.h"
+
+using namespace mbostock;
+
+static const float centerOffset = .1f;
+
+Seesaw::Seesaw(const Vector& min, const Vector& max, float mass)
+    : origin_((min + max) / 2.f), size_(max - min), drag_(1.f),
+      model_(box_) {
+  left_.inverseMass = 1.f / (mass * .1f);
+  right_.inverseMass = 1.f / (mass * .1f);
+  center_.inverseMass = 1.f / (mass * .8f);
+  reset();
+}
+
+const Shape& Seesaw::shape() const {
+  return box_;
+}
+
+Model& Seesaw::model() {
+  return model_;
+}
+
+void Seesaw::resetForces() {
+  left_.force = Vector::ZERO();
+  right_.force = Vector::ZERO();
+  center_.force = Vector::ZERO();
+  drag_.apply(left_);
+  drag_.apply(right_);
+  drag_.apply(center_);
+}
+
+void Seesaw::applyForce(UnaryForce& force) {
+  force.apply(left_);
+  force.apply(right_);
+  force.apply(center_);
+}
+
+void Seesaw::applyWeight(float w, const Vector& x) {
+  if (x.x < origin_.x) {
+    left_.force.y -= w * (2 * (origin_.x - x.x) / size_.x);
+  } else {
+    right_.force.y += w * (2 * (origin_.x - x.x) / size_.x);
+  }
+}
+
+void Seesaw::step(const ParticleSimulator& s) {
+  s.step(left_);
+  s.step(right_);
+  s.step(center_);
+  updateBox();
+}
+
+void Seesaw::updateBox() {
+  Vector x = (right_.position - left_.position) / size_.x;
+  Vector y = -x.cross(Vector::Z()) * size_.y / 2.f;
+  Vector z = Vector::Z() * size_.z / 2.f;
+  box_ = Box(
+      left_.position - z - y,
+      right_.position - z - y,
+      right_.position + z - y,
+      left_.position + z - y,
+      right_.position - z + y,
+      left_.position - z + y,
+      left_.position + z + y,
+      right_.position + z + y);
+}
+
+void Seesaw::constrainInternal() {
+  Vector v = origin_ - (left_.position + right_.position) / 2.f;
+  right_.position += v;
+  left_.position += v;
+
+  Constraints::distance(right_, origin_, size_.x / 2.f);
+  Constraints::distance(left_, origin_, size_.x / 2.f);
+  Constraints::distance(right_, left_, size_.x);
+  Constraints::distance(center_, origin_, centerOffset);
+
+  float d = sqrtf(size_.x * size_.x / 4.f + centerOffset * centerOffset);
+  Constraints::distance(left_, center_, d);
+  Constraints::distance(right_, center_, d);
+}
+
+void Seesaw::reset() {
+  left_.position = origin_;
+  left_.position.x -= size_.x / 2.f;
+  right_.position = origin_;
+  right_.position.x += size_.x / 2.f;
+  center_.position = origin_;
+  center_.position.y -= centerOffset;
+  left_.previousPosition = left_.position;
+  right_.previousPosition = right_.position;
+  center_.previousPosition = center_.position;
+  updateBox();
+}
+
+void Seesaw::setMaterial(const Material& m) {
+  model_.setMaterial(m);
+}
+
+void Seesaw::setTopMaterial(const Material& m) {
+  model_.setTopMaterial(m);
+}
+
+float Seesaw::slip() const {
+  return model_.material().slip();
+}