Initial commit.

physics/constraint.cpp

@@ -0,0 +1,143 @@
+// -*- C++ -*-
+
+#include <stdio.h>
+
+#include "constraint.h"
+#include "particle.h"
+#include "shape.h"
+
+using namespace mbostock;
+
+static Projection p;
+
+bool Constraints::minDistance(Particle& a, Vector p, float d) {
+  Vector v = p - a.position;
+  float l = v.length();
+  if (l < d) {
+    a.position += v * ((l - d) / l);
+    return true;
+  }
+  return false;
+}
+
+bool Constraints::minDistance(Particle& a, Particle& b, float d) {
+  Vector v = b.position - a.position;
+  float l = v.length();
+  if (l < d) {
+    v *= (l - d) / ((a.inverseMass + b.inverseMass) * l);
+    a.position += v * a.inverseMass;
+    b.position -= v * b.inverseMass;
+    return true;
+  }
+  return false;
+}
+
+bool Constraints::maxDistance(Particle& a, Vector p, float d) {
+  Vector v = p - a.position;
+  float l = v.length();
+  if (l > d) {
+    a.position += v * ((l - d) / l);
+    return true;
+  }
+  return false;
+}
+
+bool Constraints::maxDistance(Particle& a, Particle& b, float d) {
+  Vector v = b.position - a.position;
+  float l = v.length();
+  if (l > d) {
+    v *= (l - d) / ((a.inverseMass + b.inverseMass) * l);
+    a.position += v * a.inverseMass;
+    b.position -= v * b.inverseMass;
+    return true;
+  }
+  return false;
+}
+
+bool Constraints::distance(Particle& a, Vector p, float d) {
+  Vector v = p - a.position;
+  float l = v.length();
+  a.position += v * ((l - d) / l);
+  return l > 0.f;
+}
+
+bool Constraints::distance(Particle& a, Particle& b, float d) {
+  Vector v = b.position - a.position;
+  float l = v.length();
+  v *= (l - d) / ((a.inverseMass + b.inverseMass) * l);
+  a.position += v * a.inverseMass;
+  b.position -= v * b.inverseMass;
+  return l > 0.f;
+}
+
+bool Constraints::plane(Particle& a, Vector p, Vector n) {
+  float d = n.dot(a.position - p);
+  if (d < 0) {
+    a.position -= n * d;
+    return true;
+  }
+  return false;
+}
+
+bool Constraints::plane(Particle& a, Vector p, Vector n, float kr) {
+  float d = n.dot(a.position - p);
+  if (d < 0) {
+    Vector v = a.position - a.previousPosition;
+    Vector vNormal = n * n.dot(v);
+    Vector vTangent = v - vNormal;
+    a.position -= n * d;
+    a.previousPosition = a.position - vTangent + vNormal * kr;
+    return true;
+  }
+  return false;
+}
+
+/* XXX What if p.length is 0? Use the normal? */
+
+bool Constraints::inside(Particle& a, const Shape& s, float r) {
+  p = s.project(a.position);
+  if (p.length > -r) {
+    a.position = p.x + (p.x - a.position) * (r / p.length);
+    return true;
+  }
+  return false;
+}
+
+bool Constraints::inside(Particle& a, const Shape& s, float r, float kr) {
+  p = s.project(a.position);
+  if (p.length > -r) {
+    Vector v = a.position - a.previousPosition;
+    Vector vNormal = p.normal * p.normal.dot(v);
+    Vector vTangent = v - vNormal;
+    a.position = p.x + (p.x - a.position) * (r / p.length);
+    a.previousPosition = a.position - vTangent + vNormal * kr;
+    return true;
+  }
+  return false;
+}
+
+bool Constraints::outside(Particle& a, const Shape& s, float r) {
+  p = s.project(a.position);
+  if (p.length < r) {
+    a.position = p.x - (p.x - a.position) * (r / p.length);
+    return true;
+  }
+  return false;
+}
+
+bool Constraints::outside(Particle& a, const Shape& s, float r, float kr) {
+  p = s.project(a.position);
+  if (p.length < r) {
+    Vector v = a.position - a.previousPosition;
+    Vector vNormal = p.normal * p.normal.dot(v);
+    Vector vTangent = v - vNormal;
+    a.position = p.x - (p.x - a.position) * (r / p.length);
+    a.previousPosition = a.position - vTangent + vNormal * kr;
+    return true;
+  }
+  return false;
+}
+
+const Projection& Constraints::projection() {
+  return p;
+}

physics/constraint.h

@@ -0,0 +1,125 @@
+// -*- C++ -*-
+
+#ifndef MBOSTOCK_CONSTRAINT_H
+#define MBOSTOCK_CONSTRAINT_H
+
+#include "shape.h"
+#include "vector.h"
+
+namespace mbostock {
+
+  class Particle;
+  class Shape;
+
+  class Constraints {
+  public:
+
+    /**
+     * Constrains a particle a to be distance d from position p, independent of
+     * mass.
+     */
+    static bool distance(Particle& a, Vector p, float d);
+
+    /**
+     * Constrains two particles a and b to be distance d apart. The particles
+     * are moved in opposite directions according to their relative mass, either
+     * towards or away from each other, depending on whether their actual
+     * distance is greater or less than the specified distance d.
+     */
+    static bool distance(Particle& a, Particle& b, float d);
+
+    /**
+     * Constrains a particle a to be at least distance d from position p,
+     * independent of mass.
+     */
+    static bool minDistance(Particle& a, Vector p, float d);
+
+    /**
+     * Constrains two particles a and b to be at least distance d apart. The
+     * particles are moved in opposite directions according to their relative
+     * mass, away from each other, if their actual distance is less than the
+     * specified distance d.
+     */
+    static bool minDistance(Particle& a, Particle& b, float d);
+
+    /**
+     * Constrains a particle a to be at most distance d from position p,
+     * independent of mass.
+     */
+    static bool maxDistance(Particle& a, Vector p, float d);
+
+    /**
+     * Constrains two particles a and b to be at most distance d apart. The
+     * particles are moved in opposite directions according to their relative
+     * mass, towards each other, if their actual distance is greater than the
+     * specified distance d.
+     */
+    static bool maxDistance(Particle& a, Particle& b, float d);
+
+    /**
+     * Constrains the particle so that its y-value is at least min. This
+     * implicitly uses a coefficient of restitution of zero.
+     */
+    static bool minY(Particle& a, float min) {
+      return plane(a, Vector(0, min, 0), Vector(0, 1, 0));
+    }
+
+    /**
+     * Constrains the particle so that its y-value is at least min, simulating a
+     * collision with a plane of coefficient of restitution kr. Note that the
+     * bounce can only be simulated if the particle was previously above the
+     * plane.
+     */
+    static bool minY(Particle& a, float min, float kr) {
+      return plane(a, Vector(0, min, 0), Vector(0, 1, 0), kr);
+    }
+
+    /**
+     * Constrains the particle to be above the plane defined by the point p and
+     * the normal n. This implicitly uses a coefficient of restitution of zero.
+     */
+    static bool plane(Particle& a, Vector p, Vector n);
+
+    /**
+     * Constrains the particle to be above the plane defined by the point p and
+     * the normal n, using a coefficient of restitution kr.
+     */
+    static bool plane(Particle& a, Vector p, Vector n, float kr);
+
+    /**
+     * Constrains the particle to be inside the specified shape by at least
+     * radius r. This implicitly uses a coefficient of restitution of zero.
+     */
+    static bool inside(Particle& a, const Shape& s, float r);
+
+    /**
+     * Constrains the particle to be inside the specified shape by at least
+     * radius r, using a coefficient of restitution kr.
+     */
+    static bool inside(Particle& a, const Shape& s, float r, float kr);
+
+    /**
+     * Constrains the particle to be outside the specified shape by at least
+     * radius r. This implicitly uses a coefficient of restitution of zero.
+     */
+    static bool outside(Particle& a, const Shape& s, float r);
+
+    /**
+     * Constrains the particle to be outside the specified shape by at least
+     * radius r, using a coefficient of restitution kr.
+     */
+    static bool outside(Particle& a, const Shape& s, float r, float kr);
+
+    /**
+     * Returns the projection information for the last call to a shape-based
+     * constraint.
+     */
+    static const Projection& projection();
+
+  private:
+    Constraints();
+  };
+
+}
+
+#endif

physics/force.cpp

@@ -0,0 +1,94 @@
+// -*- C++ -*-
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "force.h"
+#include "particle.h"
+#include "vector.h"
+
+using namespace mbostock;
+
+static const float epsilon = 1E-20;
+
+void UnaryForce::apply(Particle& p) {
+  p.force += force(p);
+}
+
+void BinaryForce::apply(Particle& a, Particle& b) {
+  Vector f = force(a, b);
+  a.force += f;
+  b.force -= f;
+}
+
+RandomForce::RandomForce(float k)
+    : k_(k) {
+}
+
+Vector RandomForce::force(Particle& p) {
+  return Vector::randomVector(k_);
+}
+
+GravitationalForce::GravitationalForce(float g)
+    : g_(g) {
+}
+
+Vector GravitationalForce::force(const Particle& p) {
+  return Vector(0, -g_ / p.inverseMass, 0);
+}
+
+LinearDragForce::LinearDragForce(float kd)
+    : kd_(kd) {
+}
+
+Vector LinearDragForce::force(const Particle& p) {
+  return p.velocity() * -kd_;
+}
+
+QuadraticDragForce::QuadraticDragForce(float kd)
+    : kd_(kd) {
+}
+
+Vector QuadraticDragForce::force(const Particle& p) {
+  const Vector& v = p.velocity();
+  return v * v.length() * -kd_;
+}
+
+HookeForce::HookeForce(float r, float ks)
+    : r_(r), ks_(ks) {
+}
+
+Vector HookeForce::force(const Particle& a, const Particle& b) {
+  Vector l = a.position - b.position;
+  float ll = l.length();
+  if (ll == 0.f) {
+    l = Vector::randomVector(epsilon);
+    ll = epsilon;
+  }
+  return l * -ks_ * (ll - r_) / ll;
+}
+
+DampenedHookeForce::DampenedHookeForce(float r, float ks, float kd)
+    : r_(r), ks_(ks), kd_(kd) {
+}
+
+Vector DampenedHookeForce::force(const Particle& a, const Particle& b) {
+  Vector l = a.position - b.position;
+  Vector dl = a.velocity() - b.velocity();
+  float ll = l.length();
+  if (ll == 0.f) {
+    l = Vector::randomVector(epsilon);
+    ll = epsilon;
+  }
+  return l * -(ks_ * (ll - r_) + kd_ * dl.dot(l) / ll) / ll;
+}
+
+CoulombForce::CoulombForce(float e)
+    : e_(e) {
+}
+
+Vector CoulombForce::force(const Particle& a, const Particle& b) {
+  Vector l = a.position - b.position;
+  float ll = l.length();
+  return -l * e_ / (ll * ll * ll);
+}

physics/force.h

@@ -0,0 +1,105 @@
+// -*- C++ -*-
+
+#ifndef MBOSTOCK_FORCE_H
+#define MBOSTOCK_FORCE_H
+
+#include "vector.h"
+
+namespace mbostock {
+
+  class Particle;
+
+  class UnaryForce {
+  public:
+    virtual ~UnaryForce() {}
+
+    void apply(Particle& p);
+
+    virtual Vector force(const Particle& p)  = 0;
+  };
+
+  class BinaryForce {
+  public:
+    virtual ~BinaryForce() {}
+
+    void apply(Particle& a, Particle& b);
+
+    virtual Vector force(const Particle& a, const Particle& b) = 0;
+  };
+
+  class RandomForce : public UnaryForce {
+  public:
+    RandomForce(float k);
+
+    virtual Vector force(Particle& p);
+
+  private:
+    float k_;
+  };
+
+  class GravitationalForce : public UnaryForce {
+  public:
+    GravitationalForce(float g);
+
+    virtual Vector force(const Particle& p);
+
+  private:
+    float g_;
+  };
+
+  class LinearDragForce : public UnaryForce {
+  public:
+    LinearDragForce(float kd);
+
+    virtual Vector force(const Particle& p);
+
+  private:
+    float kd_;
+  };
+
+  class QuadraticDragForce : public UnaryForce {
+  public:
+    QuadraticDragForce(float kd);
+
+    virtual Vector force(const Particle& p);
+
+  private:
+    float kd_;
+  };
+
+  class HookeForce : public BinaryForce {
+  public:
+    HookeForce(float r, float ks);
+
+    virtual Vector force(const Particle& a, const Particle& b);
+
+  private:
+    float r_;
+    float ks_;
+  };
+
+  class DampenedHookeForce : public BinaryForce {
+  public:
+    DampenedHookeForce(float r, float ks, float kd);
+
+    virtual Vector force(const Particle& a, const Particle& b);
+
+  private:
+    float r_;
+    float ks_;
+    float kd_;
+  };
+
+  class CoulombForce : public BinaryForce {
+  public:
+    CoulombForce(float e);
+
+    virtual Vector force(const Particle& a, const Particle& b);
+
+  private:
+    float e_;
+  };
+
+}
+
+#endif

physics/particle.cpp

@@ -0,0 +1,37 @@
+// -*- C++ -*-
+
+#include <stdio.h>
+#include <stdlib.h>
+
+#include "particle.h"
+
+using namespace mbostock;
+
+Particle::Particle() : inverseMass(1.f) {
+}
+
+Vector Particle::velocity() const {
+  /* Note: ignores force * (inverseMass * timeStep / 2). */
+  return (position - previousPosition) / ParticleSimulator::timeStep();
+}
+
+ParticleSimulator::ParticleSimulator()
+    : drag_(1.f) {
+}
+
+ParticleSimulator::ParticleSimulator(float drag) 
+    : drag_(drag) {
+}
+
+float ParticleSimulator::timeStep() {
+  return .003f; // TODO .005 and interpolate
+}
+
+void ParticleSimulator::step(Particle& p) const {
+  static const float timeStepSquared = timeStep() * timeStep();
+
+  Vector p0 = p.previousPosition;
+  p.previousPosition = p.position;
+  p.position += (p.position - p0) * drag_
+      + p.force * (p.inverseMass * timeStepSquared);
+}

physics/particle.h

@@ -0,0 +1,39 @@
+// -*- C++ -*-
+
+#ifndef MBOSTOCK_PARTICLE_H
+#define MBOSTOCK_PARTICLE_H
+
+#include "vector.h"
+
+namespace mbostock {
+
+  class Particle {
+  public:
+    Particle();
+
+    float inverseMass;
+    Vector position;
+    Vector previousPosition;
+    Vector force;
+
+    Vector velocity() const;
+  };
+
+  class ParticleSimulator {
+  public:
+    ParticleSimulator();
+    ParticleSimulator(float drag);
+
+    void step(Particle& p) const;
+
+    static float timeStep();
+
+  private:
+    static float timeStep_;
+    static float timeStepSquared_;
+    float drag_;
+  };
+
+}
+
+#endif

physics/rotation.cpp

@@ -0,0 +1,89 @@
+// -*- C++ -*-
+
+#include <math.h>
+
+#include "particle.h"
+#include "rotation.h"
+
+using namespace mbostock;
+
+Rotation::Rotation(const Vector& origin, const Vector& axis,
+                   float speed, float angle)
+    : origin_(origin), axis_(axis), startAngle_(angle),
+      speed_(speed * ParticleSimulator::timeStep()), angle_(angle) {
+  update();
+}
+
+void Rotation::reset() {
+  angle_ = startAngle_;
+}
+
+void Rotation::step() {
+  if (!enabled()) {
+    return;
+  }
+  angle_ += speed_;
+  angle_ = fmodf(angle_, 360.f);
+  update();
+}
+
+void Rotation::update() {
+  float r = angle_ * (2.f * M_PI / 360.f);
+  float c = cosf(r);
+  float s = sinf(r);
+
+  /* This look familiar to anyone? /wink */
+  matrix_[0] = axis_.x * axis_.x * (1.f - c) + c;
+  matrix_[1] = axis_.x * axis_.y * (1.f - c) - axis_.z * s;
+  matrix_[2] = axis_.x * axis_.z * (1.f - c) + axis_.y * s;
+  matrix_[3] = axis_.y * axis_.x * (1.f - c) + axis_.z * s;
+  matrix_[4] = axis_.y * axis_.y * (1.f - c) + c;
+  matrix_[5] = axis_.y * axis_.z * (1.f - c) - axis_.x * s;
+  matrix_[6] = axis_.x * axis_.z * (1.f - c) - axis_.y * s;
+  matrix_[7] = axis_.y * axis_.z * (1.f - c) + axis_.x * s;
+  matrix_[8] = axis_.z * axis_.z * (1.f - c) + c;
+}
+
+Vector Rotation::point(const Vector& x) const {
+  return origin_ + vector(x - origin_);
+}
+
+Vector Rotation::pointInverse(const Vector& x) const {
+  return origin_ + vectorInverse(x - origin_);
+}
+
+Vector Rotation::vector(const Vector& x) const {
+  return Vector(
+      matrix_[0] * x.x + matrix_[1] * x.y + matrix_[2] * x.z,
+      matrix_[3] * x.x + matrix_[4] * x.y + matrix_[5] * x.z,
+      matrix_[6] * x.x + matrix_[7] * x.y + matrix_[8] * x.z);
+}
+
+Vector Rotation::vectorInverse(const Vector& x) const {
+  return Vector(
+      matrix_[0] * x.x + matrix_[3] * x.y + matrix_[6] * x.z,
+      matrix_[1] * x.x + matrix_[4] * x.y + matrix_[7] * x.z,
+      matrix_[2] * x.x + matrix_[5] * x.y + matrix_[8] * x.z);
+}
+
+Vector Rotation::velocity(const Vector& x) const {
+  return enabled()
+      ? (origin_ - x).cross(axis_) * speed_ * (2.f * M_PI / 360.f)
+      : Vector::ZERO();
+}
+
+RotatingShape::RotatingShape(const Shape& s, const Rotation& r)
+    : shape_(s), rotation_(r) {
+}
+
+bool RotatingShape::intersects(const Sphere& s) const {
+  Sphere rs(rotation_.pointInverse(s.x()), s.radius());
+  return shape_.intersects(rs);
+}
+
+Projection RotatingShape::project(const Vector& x) const {
+  Projection p = shape_.project(rotation_.pointInverse(x));
+  p.x = rotation_.point(p.x);
+  p.normal = rotation_.vector(p.normal);
+  return p;
+}

physics/rotation.h

@@ -0,0 +1,79 @@
+// -*- C++ -*-
+
+#ifndef MBOSTOCK_ROTATION_H
+#define MBOSTOCK_ROTATION_H
+
+#include "shape.h"
+#include "vector.h"
+#include "transform.h"
+
+namespace mbostock {
+
+  /**
+   * Computes a rotation matrix for a given origin, axis and angle. This is
+   * roughly the equivalent to a glTranslatef and glRotatef call in OpenGL.
+   */
+  class Rotation : public Transform {
+  public:
+    Rotation(const Vector& origin, const Vector& axis,
+             float speed, float angle);
+
+    /** Rotates the specified point. */
+    Vector point(const Vector& x) const;
+
+    /** Inverse-rotates the specified point. */
+    Vector pointInverse(const Vector& x) const;
+
+    /** Rotates the specified vector. */
+    Vector vector(const Vector& x) const;
+
+    /** Inverse-rotates the specified vector. */
+    Vector vectorInverse(const Vector& x) const;
+
+    /** Returns the rotation's origin. */
+    inline const Vector& origin() const { return origin_; }
+
+    /** Returns the rotation's axis. */
+    inline const Vector& axis() const { return axis_; }
+
+    /** Returns the rotation's angle (in degrees, in the range [0, 360]). */
+    inline float angle() const { return angle_; }
+
+    /** Returns the speed of the rotation (in degrees per time step). */
+    inline float speed() const { return speed_; }
+
+    /** Advances the rotation by one time step. */
+    virtual void step();
+
+    /** Resets the rotation. */
+    virtual void reset();
+
+    /** Returns the velocity of the given point. */
+    Vector velocity(const Vector& x) const;
+
+  private:
+    void update();
+
+    Vector origin_;
+    Vector axis_;
+    float startAngle_;
+    float angle_;
+    float speed_;
+    float matrix_[9];
+  };
+
+  class RotatingShape : public Shape {
+  public:
+    RotatingShape(const Shape& s, const Rotation& r);
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& x) const;
+
+  private:
+    const Shape& shape_;
+    const Rotation& rotation_;
+  };
+
+}
+
+#endif

physics/shape.cpp

@@ -0,0 +1,514 @@
+// -*- C++ -*-
+
+#include <algorithm>
+#include <math.h>
+
+#include "shape.h"
+
+using namespace mbostock;
+
+Projection::Projection() : length(0) {
+}
+
+Projection::Projection(const Vector& x, float length)
+    : x(x), length(length) {
+}
+
+Projection::Projection(const Vector& x, float length, const Vector& normal)
+    : x(x), length(length), normal(normal) {
+}
+
+bool Projection::operator<(const Projection& p) const {
+  return fabsf(length) < fabsf(p.length);
+}
+
+bool Projection::operator<=(const Projection& p) const {
+  return fabsf(length) <= fabsf(p.length);
+}
+
+bool Projection::operator>(const Projection& p) const {
+  return fabsf(length) > fabsf(p.length);
+}
+
+bool Projection::operator>=(const Projection& p) const {
+  return fabsf(length) >= fabsf(p.length);
+}
+
+Sphere::Sphere()
+    : r_(0.f), r2_(0.f) {
+}
+
+Sphere::Sphere(const Vector& x, float radius)
+    : x_(x), r_(radius), r2_(radius * radius) {
+}
+
+bool Sphere::above(const Plane& p) const {
+  /* Derived from Point-Plane Distance on MathWorld. */
+  return p.n_.dot(x_ - p.x_) > r_;
+}
+
+bool Sphere::below(const Plane& p) const {
+  /* Derived from Point-Plane Distance on MathWorld. */
+  return p.n_.dot(x_ - p.x_) < -r_;
+}
+
+bool Sphere::intersects(const Sphere& s) const {
+  /* Derived from Moller-Haines section 16.13.1. */
+  float r = r_ + s.r_;
+  return (s.x_ - x_).squared() < (r * r);
+}
+
+Projection Sphere::project(const Vector& x) const {
+  Vector v = x - x_;
+  float l = v.length();
+  float d = l - r_;
+  Vector n = v / l;
+  return Projection(x - n * d, fabsf(d), (d < 0.f) ? -n : n);
+}
+
+LineSegment::LineSegment() {
+}
+
+LineSegment::LineSegment(const Vector& x0, const Vector& x1)
+    : x0_(x0), x1_(x1), x01_(x1 - x0), l2_(x01_.dot(x01_)) {
+}
+
+bool LineSegment::intersects(const Sphere& s) const {
+  /* Derived from Point-Line Distance--3-Dimensional on MathWorld. */
+  return (x01_.cross(x0_ - s.x_).squared() / l2_) < s.r2_;
+}
+
+Projection LineSegment::project(const Vector& p) const {
+  /* Derived from Point-Line Distance--3-Dimensional on MathWorld. */
+  float t = (p - x0_).dot(x01_) / l2_;
+  Vector x = (t <= 0.f) ? x0_ : ((t >= 1.f) ? x1_ : (x0_ + x01_ * t));
+  return Projection(x, (p - x).length());
+}
+
+Plane::Plane() {
+}
+
+Plane::Plane(const Vector& x, const Vector& normal)
+    : x_(x), n_(normal) {
+}
+
+bool Plane::intersects(const Sphere& s) const {
+  /* Derived from Point-Plane Distance on MathWorld. */
+  return fabsf(n_.dot(s.x_ - x_)) < s.r_;
+}
+
+Projection Plane::project(const Vector& p) const {
+  /* Derived from Point-Plane Distance on MathWorld. */
+  float l = n_.dot(p - x_);
+  return Projection(p - n_ * l, fabsf(l), (l < 0.f) ? -n_ : n_);
+}
+
+Triangle::Triangle() {
+}
+
+Triangle::Triangle(const Vector& x0, const Vector& x1, const Vector& x2)
+    : x01_(x0, x1), x12_(x1, x2), x20_(x2, x0),
+      p_(x0, (x1 - x0).cross(x2 - x1).normalize()) {
+}
+
+bool Triangle::intersects(const Sphere& s) const {
+  /* Derived from ERIT section 2.3 (reordered). */
+  Projection p0 = p_.project(s.x_);
+  if (p0.length > s.r_) {
+    return false;
+  }
+  if (x01_.intersects(s) || x12_.intersects(s) || x20_.intersects(s)) {
+    return true;
+  }
+  return contains(p0.x);
+}
+
+Projection Triangle::project(const Vector& p) const {
+  Projection pp = p_.project(p);
+  if (contains(pp.x)) {
+    return pp;
+  }
+  Projection p01 = x01_.project(pp.x);
+  Projection p12 = x12_.project(pp.x);
+  Projection p20 = x20_.project(pp.x);
+  Projection pb = std::min(std::min(p01, p12), p20);
+  pb.length = sqrtf(pb.length * pb.length + pp.length * pp.length);
+  pb.normal = pp.normal;
+  return pb;
+}
+
+bool Triangle::contains(const Vector& p) const {
+  return ((x0() - x1()).cross(p - x0()).dot(p_.normal()) < 0.f)
+      && ((x1() - x2()).cross(p - x1()).dot(p_.normal()) < 0.f)
+      && ((x2() - x0()).cross(p - x2()).dot(p_.normal()) < 0.f);
+}
+
+Quad::Quad() {
+}
+
+Quad::Quad(const Vector& x0, const Vector& x1,
+           const Vector& x2, const Vector& x3)
+    : x01_(x0, x1), x12_(x1, x2), x23_(x2, x3), x30_(x3, x0),
+      p_(x0, (x1 - x0).cross(x2 - x1).normalize()) {
+}
+
+bool Quad::intersects(const Sphere& s) const {
+  /* Derived from Triangle-Sphere test above. */
+  Projection p0 = p_.project(s.x());
+  if (p0.length > s.radius()) {
+    return false;
+  }
+  if (x01_.intersects(s)
+      || x12_.intersects(s)
+      || x23_.intersects(s)
+      || x30_.intersects(s)) {
+    return true;
+  }
+  return contains(p0.x);
+}
+
+Projection Quad::project(const Vector& p) const {
+  Projection pp = p_.project(p);
+  if (contains(pp.x)) {
+    return pp;
+  }
+  Projection p01 = x01_.project(pp.x);
+  Projection p12 = x12_.project(pp.x);
+  Projection p23 = x23_.project(pp.x);
+  Projection p30 = x30_.project(pp.x);
+  Projection pb = std::min(std::min(std::min(p01, p12), p23), p30);
+  pb.length = sqrtf(pb.length * pb.length + pp.length * pp.length);
+  pb.normal = pp.normal;
+  return pb;
+}
+
+bool Quad::contains(const Vector& p) const {
+  return ((x0() - x1()).cross(p - x0()).dot(p_.normal()) < 0.f)
+      && ((x1() - x2()).cross(p - x1()).dot(p_.normal()) < 0.f)
+      && ((x2() - x3()).cross(p - x2()).dot(p_.normal()) < 0.f)
+      && ((x3() - x0()).cross(p - x3()).dot(p_.normal()) < 0.f);
+}
+
+Wedge::Wedge() {
+}
+
+Wedge::Wedge(const Vector& x0, const Vector& x1,
+             const Vector& x2, const Vector& x3)
+    : top_(x0, x1, x2, x3),
+      right_(x2, x1, Vector(x1.x, x0.y, x1.z), Vector(x2.x, x0.y, x2.z)),
+      bottom_(x0, x3, right_.x3(), right_.x2()),
+      front_(x1, x0, right_.x2()),
+      back_(x3, x2, right_.x3()) {
+}
+
+bool Wedge::intersects(const Sphere& s) const {
+  /* Derived (very approximately) from Moller-Haines section 16.14.2. */
+  if (s.above(top_.p_)
+      || s.above(right_.p_)
+      || s.above(front_.p_)
+      || s.above(back_.p_)
+      || s.above(bottom_.p_)) {
+    return false;
+  }
+  if (s.below(top_.p_)
+      && s.below(right_.p_)
+      && s.below(front_.p_)
+      && s.below(back_.p_)
+      && s.below(bottom_.p_)) {
+    return true;
+  }
+  return top_.intersects(s)
+    || right_.intersects(s)
+    || front_.intersects(s)
+    || back_.intersects(s)
+    || bottom_.intersects(s);
+}
+
+Projection Wedge::project(const Vector& p) const {
+  Projection pt = top_.project(p);
+  Projection pr = right_.project(p);
+  Projection pf = front_.project(p);
+  Projection pb = back_.project(p);
+  Projection pd = bottom_.project(p);
+  return std::min(std::min(std::min(std::min(pt, pr), pf), pb), pd);
+}
+
+AxisAlignedBox::AxisAlignedBox() {
+}
+
+AxisAlignedBox::AxisAlignedBox(const Vector& min, const Vector& max)
+    : min_(min), max_(max) {
+}
+
+bool AxisAlignedBox::intersects(const Sphere& s) const {
+  /* Derived from Moller-Haines section 16.13.2. */
+  Vector e = Vector::max(min_ - s.x_, Vector::ZERO())
+           + Vector::max(s.x_ - max_, Vector::ZERO());
+  return e.squared() < s.r2_;
+}
+
+bool AxisAlignedBox::contains(const Vector& p) const {
+  return ((p.x >= min_.x) && (p.x < max_.x)
+          && (p.y >= min_.y) && (p.y < max_.y)
+          && (p.z >= min_.z) && (p.z < max_.z));
+}
+
+Projection AxisAlignedBox::project(const Vector& p) const {
+  return contains(p) ? projectOut(p) : projectIn(p);
+}
+
+Projection AxisAlignedBox::projectOut(const Vector& p) const {
+  Vector x = p;
+  Vector n;
+  float l;
+
+  float dx1 = p.x - min_.x;
+  float dx2 = max_.x - p.x;
+  float dx = std::min(dx1, dx2);
+
+  float dy1 = p.y - min_.y;
+  float dy2 = max_.y - p.y;
+  float dy = std::min(dy1, dy2);
+
+  float dz1 = p.z - min_.z;
+  float dz2 = max_.z - p.z;
+  float dz = std::min(dz1, dz2);
+
+  if ((dx <= dy) && (dx <= dz)) {
+    if (dx1 <= dx2) {
+      l = dx1;
+      x.x = min_.x;
+      n.x = 1.f;
+    } else {
+      l = dx2;
+      x.x = max_.x;
+      n.x = -1.f;
+    }
+  } else if (dy <= dz) {
+    if (dy1 <= dy2) {
+      l = dy1;
+      x.y = min_.y;
+      n.y = 1.f;
+    } else {
+      l = dy2;
+      x.y = max_.y;
+      n.y = -1.f;
+    }
+  } else {
+    if (dz1 <= dz2) {
+      l = dz1;
+      x.z = min_.z;
+      n.z = 1.f;
+    } else {
+      l = dz2;
+      x.z = max_.z;
+      n.z = -1.f;
+    }
+  }
+
+  return Projection(x, -l, n);
+}
+
+Projection AxisAlignedBox::projectIn(const Vector& p) const {
+  Vector x = p;
+  Vector n;
+
+  if (p.x < min_.x) {
+    x.x = min_.x;
+    n = -Vector::X();
+  } else if (p.x >= max_.x) {
+    x.x = max_.x;
+    n = Vector::X();
+  }
+
+  if (p.z < min_.z) {
+    x.z = min_.z;
+    n = -Vector::Z();
+  } else if (p.z >= max_.z) {
+    x.z = max_.z;
+    n = Vector::Z();
+  }
+
+  if (p.y < min_.y) {
+    x.y = min_.y;
+    n = -Vector::Y();
+  } else if (p.y >= max_.y) {
+    x.y = max_.y;
+    n = Vector::Y();
+  }
+
+  /*
+   * TODO: If the particle does not project directly onto the triangle (and
+   * similarly for quads), we should interpolate the normal of the projection so
+   * that particles appear to bounce off the corner.
+   *
+   * On the other hand, this leads to some weird behavior when applying
+   * directional friction when Polly is on the edge of a platform. So for now,
+   * just use axis normals.
+   */
+
+  return Projection(x, (p - x).length(), n);
+}
+
+Box::Box() {
+}
+
+Box::Box(const AxisAlignedBox& box)
+    : top_(box.x4(), box.x5(), box.x6(), box.x7()),
+      bottom_(box.x0(), box.x1(), box.x2(), box.x3()),
+      left_(box.x0(), box.x3(), box.x6(), box.x5()),
+      right_(box.x1(), box.x4(), box.x7(), box.x2()),
+      front_(box.x2(), box.x7(), box.x6(), box.x3()),
+      back_(box.x0(), box.x5(), box.x4(), box.x1()) {
+}
+
+Box::Box(const Vector& c, const Vector& x, const Vector& y, const Vector& z) {
+  Vector x0 = c - x - y - z;
+  Vector x1 = c + x - y - z;
+  Vector x2 = c + x - y + z;
+  Vector x3 = c - x - y + z;
+  Vector x4 = c + x + y - z;
+  Vector x5 = c - x + y - z;
+  Vector x6 = c - x + y + z;
+  Vector x7 = c + x + y + z;
+  top_ = Quad(x4, x5, x6, x7);
+  bottom_ = Quad(x0, x1, x2, x3);
+  left_ = Quad(x0, x3, x6, x5);
+  right_ = Quad(x1, x4, x7, x2);
+  front_ = Quad(x2, x7, x6, x3);
+  back_ = Quad(x0, x5, x4, x1);
+}
+
+Box::Box(const Vector& x0, const Vector& x1, const Vector& x2, const Vector& x3,
+         const Vector& x4, const Vector& x5, const Vector& x6, const Vector& x7)
+    : top_(x4, x5, x6, x7),
+      bottom_(x0, x1, x2, x3),
+      left_(x0, x3, x6, x5),
+      right_(x1, x4, x7, x2),
+      front_(x2, x7, x6, x3),
+      back_(x0, x5, x4, x1) {
+}
+
+bool Box::intersects(const Sphere& s) const {
+  /* Derived (very approximately) from Moller-Haines section 16.14.2. */
+  if (s.above(top_.plane())
+      || s.above(left_.plane())
+      || s.above(right_.plane())
+      || s.above(front_.plane())
+      || s.above(back_.plane())
+      || s.above(bottom_.plane())) {
+    return false;
+  }
+  if (s.below(top_.plane())
+      && s.below(left_.plane())
+      && s.below(right_.plane())
+      && s.below(front_.plane())
+      && s.below(back_.plane())
+      && s.below(bottom_.plane())) {
+    return true;
+  }
+  return top_.intersects(s)
+    || left_.intersects(s)
+    || right_.intersects(s)
+    || front_.intersects(s)
+    || back_.intersects(s)
+    || bottom_.intersects(s);
+}
+
+Projection Box::project(const Vector& v) const {
+  Projection p = top_.project(v);
+  p = std::min(p, bottom_.project(v));
+  p = std::min(p, left_.project(v));
+  p = std::min(p, right_.project(v));
+  p = std::min(p, front_.project(v));
+  p = std::min(p, back_.project(v));
+  return p;
+}
+
+Cylinder::Cylinder() {
+}
+
+Cylinder::Cylinder(const Vector& x0, const Vector& x1, float radius)
+    : axis_(x0, x1), l_(sqrtf(axis_.l2_)), r_(radius), v_(axis_.x01_ / l_) {
+}
+
+bool Cylinder::intersects(const Sphere& s) const {
+  /* Derived from ERIT section 2.5. */
+  Vector pa = s.x_ - axis_.x0_;
+  float pqdotpa = axis_.x01_.dot(pa);
+  float rs1;
+
+  /* P -> Q -> B */
+  if (pqdotpa > axis_.l2_) {
+    float f = pqdotpa - axis_.l2_;
+    float d2 = (f * f) / axis_.l2_;
+    if (d2 > s.r2_) {
+      return false;
+    }
+    rs1 = sqrtf(s.r2_ - d2);
+  }
+
+  /* B -> P -> Q */
+  else if (pqdotpa < 0.f) {
+    float qpdotqa = -axis_.x01_.dot(s.x_ - axis_.x1_);
+    float f = qpdotqa - axis_.l2_;
+    float d2 = (f * f) / axis_.l2_;
+    if (d2 > s.r2_) {
+      return false;
+    }
+    rs1 = sqrtf(s.r2_ - d2);
+  }
+
+  /* P -> B -> Q */
+  else {
+    rs1 = s.r_;
+  }
+
+  float ld2 = axis_.x01_.cross(pa).squared() / axis_.l2_;
+  float r = r_ + rs1;
+  return ld2 <= (r * r);
+}
+
+Projection Cylinder::project(const Vector& p) const {
+  /* Derived from ERIT section 2.5. */
+  Vector pa = p - axis_.x0_;
+  float pqdotpa = axis_.x01_.dot(pa);
+
+  /* P -> Q -> B */
+  if (pqdotpa > axis_.l2_) {
+    Vector x = Plane(axis_.x1_, v_).project(p).x;
+
+    Vector v = x - axis_.x1_;
+    float l = v.length();
+    if (l > r_) {
+      float d = l - r_;
+      Vector n = v / l;
+      x -= n * d;
+    }
+
+    return Projection(x, (p - x).length(), v_);
+  }
+
+  /* B -> P -> Q */
+  else if (pqdotpa < 0.f) {
+    Vector x = Plane(axis_.x0_, v_).project(p).x;
+
+    Vector v = x - axis_.x0_;
+    float l = v.length();
+    if (l > r_) {
+      float d = l - r_;
+      Vector n = v / l;
+      x -= n * d;
+    }
+
+    return Projection(x, (p - x).length(), -v_);
+  }
+
+  /* P -> B -> Q */
+  Vector b = axis_.x0_ + axis_.x01_ * pqdotpa / axis_.l2_;
+  Vector v = p - b;
+  float l = v.length();
+  float d = l - r_;
+  Vector n = v / l;
+  return Projection(p - n * d, fabsf(d), (d < 0.f) ? -n : n);
+}

physics/shape.h

@@ -0,0 +1,431 @@
+// -*- C++ -*-
+
+#ifndef MBOSTOCK_SHAPE_H
+#define MBOSTOCK_SHAPE_H
+
+#include "vector.h"
+
+namespace mbostock {
+
+  class Plane;
+  class Sphere;
+
+  /**
+   * Represents the projection of a point onto the closest corresponding point
+   * on the surface of a shape. A projection is used when the bounding sphere of
+   * a particle interpenetrates a shape and, as this constitutes a collision,
+   * the sphere center is projected onto the surface of the shape to determine
+   * how to move the particle out in response to the collision.
+   *
+   * When a point is projected onto a surface, the projection includes the
+   * surface normal. However, if the point is projected onto a line, the
+   * corresponding normal is undefined.
+   *
+   * Projections define a natural order based on the length of the projection,
+   * so that it is convenient to find the shortest projection given a number of
+   * candidate projections.
+   */
+  class Projection {
+  public:
+    Projection();
+    Projection(const Vector& x, float length);
+    Projection(const Vector& x, float length, const Vector& normal);
+
+    /** The location of the projected point on the shape's surface. */
+    Vector x;
+
+    /** The distance from the original point to the projected point. */
+    float length;
+
+    /** The surface normal at the projected point. */
+    Vector normal;
+
+    bool operator<(const Projection& p) const;
+    bool operator<=(const Projection& p) const;
+    bool operator>(const Projection& p) const;
+    bool operator>=(const Projection& p) const;
+  };
+
+  /**
+   * Represents a shape in three dimensions. Since particles are represented
+   * solely as spheres, shapes need only define intersection tests with spheres,
+   * and how to project points onto the closest point on the surface of the
+   * shape.
+   */
+  class Shape {
+  public:
+    virtual ~Shape() {}
+
+    /** Returns true if the specified sphere intersects with this shape. */
+    virtual bool intersects(const Sphere& s) const = 0;
+
+    /**
+     * Projects the specified point onto the surface of the shape. The
+     * projection specifies the closest point on the surface of the shape to the
+     * specified point, the pre-computed length of the projection (the distance
+     * between two points), and the surface normal at the projected point, if
+     * defined.
+     */
+    virtual Projection project(const Vector& x) const = 0;
+  };
+
+  /** Represents a sphere using a center point and a radius. */
+  class Sphere : public Shape {
+  public:
+    Sphere();
+    Sphere(const Vector& x, float radius);
+
+    /** The center point of the sphere. */
+    inline Vector& x() { return x_; }
+    inline const Vector& x() const { return x_; }
+
+    /** The radius of the sphere. */
+    inline float radius() const { return r_; }
+
+    /** Returns true if the sphere is entirely above the specified plane. */
+    bool above(const Plane& p) const;
+
+    /** Returns true if the sphere is entirely below the specified plane. */
+    bool below(const Plane& p) const;
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& p) const;
+
+  private:
+    Vector x_;
+    float r_;
+    float r2_;
+
+    friend class AxisAlignedBox;
+    friend class Cylinder;
+    friend class LineSegment;
+    friend class Plane;
+    friend class Triangle;
+  };
+
+  /** Represents a line segment using a start point and an end point. */
+  class LineSegment : public Shape {
+  public:
+    LineSegment();
+    LineSegment(const Vector& x0, const Vector& x1);
+
+    /** The start point of the line segment. */
+    inline const Vector& x0() const { return x0_; }
+
+    /** The end point of the line segment. */
+    inline const Vector& x1() const { return x1_; }
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& p) const;
+
+  private:
+    Vector x0_;
+    Vector x1_;
+    Vector x01_;
+    float l2_;
+
+    friend class Cylinder;
+  };
+
+  /** Represents a plane using a point on the plane and a normal. */
+  class Plane : public Shape {
+  public:
+    Plane();
+    Plane(const Vector& x, const Vector& normal);
+
+    /** A point on the plane. */
+    inline const Vector& x() const { return x_; }
+
+    /** The normal. */
+    inline const Vector& normal() const { return n_; }
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& p) const;
+
+  private:
+    Vector x_;
+    Vector n_;
+
+    friend class Sphere;
+  };
+
+  /** Represents a triangle using three coplanar points. */
+  class Triangle : public Shape {
+  public:
+    Triangle();
+    Triangle(const Vector& x0, const Vector& x1, const Vector& x2);
+
+    /** The first point of the triangle. */
+    inline const Vector& x0() const { return x01_.x0(); }
+
+    /** The second point of the triangle. */
+    inline const Vector& x1() const { return x12_.x0(); }
+
+    /** The third point of the triangle. */
+    inline const Vector& x2() const { return x20_.x0(); }
+
+    /** The normal of the triangle, using counterclockwise orientation. */
+    inline const Vector& normal() const { return p_.normal(); }
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& p) const;
+
+  private:
+    bool contains(const Vector& p) const;
+
+    LineSegment x01_;
+    LineSegment x12_;
+    LineSegment x20_;
+    Plane p_;
+
+    friend class Wedge;
+  };
+
+  /** Represents a quad using four coplanar points. */
+  class Quad : public Shape {
+  public:
+    Quad();
+    Quad(const Vector& x0, const Vector& x1,
+         const Vector& x2, const Vector& x3);
+
+    /** The first point of the quad. */
+    inline const Vector& x0() const { return x01_.x0(); }
+
+    /** The second point of the quad. */
+    inline const Vector& x1() const { return x12_.x0(); }
+
+    /** The third point of the quad. */
+    inline const Vector& x2() const { return x23_.x0(); }
+
+    /** The fourth point of the quad. */
+    inline const Vector& x3() const { return x30_.x0(); }
+
+    /** The normal of the quad, using counterclockwise orientation. */
+    inline const Vector& normal() const { return p_.normal(); }
+
+    /** The plane of the quad. */
+    inline const Plane& plane() const { return p_; }
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& p) const;
+
+  private:
+    bool contains(const Vector& p) const;
+
+    LineSegment x01_;
+    LineSegment x12_;
+    LineSegment x23_;
+    LineSegment x30_;
+    Plane p_;
+
+    friend class Wedge;
+  };
+
+  /**
+   * Represents a wedge using the four coplanar points of the top surface.
+   * Points x1 and x2 define the upper edge of the wedge; points x4 and x5 lie
+   * directly beneath them to define the fifth side.
+   */
+  class Wedge : public Shape {
+  public:
+    Wedge();
+    Wedge(const Vector& x0, const Vector& x1,
+          const Vector& x2, const Vector& x3);
+
+    /** Returns the first (lower) point of the wedge. */
+    inline const Vector& x0() const { return top_.x0(); }
+
+    /** Returns the second (upper) point of the wedge. */
+    inline const Vector& x1() const { return top_.x1(); }
+
+    /** Returns the third (upper) point of the wedge. */
+    inline const Vector& x2() const { return top_.x2(); }
+
+    /** Returns the forth (upper) point of the wedge. */
+    inline const Vector& x3() const { return top_.x3(); }
+
+    /** Returns the fifth point of the wedge, below x1. */
+    inline const Vector& x4() const { return right_.x2(); }
+
+    /** Returns the six point of the wedge, below x2. */
+    inline const Vector& x5() const { return right_.x3(); }
+
+    /** Returns the top quad. */
+    inline const Quad& top() const { return top_; }
+
+    /** Returns the right quad. */
+    inline const Quad& right() const { return right_; }
+
+    /** Returns the bottom quad. */
+    inline const Quad& bottom() const { return bottom_; }
+
+    /** Returns the front triangle. */
+    inline const Triangle& front() const { return front_; }
+
+    /** Returns the back triangle. */
+    inline const Triangle& back() const { return back_; }
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& p) const;
+
+  private:
+    Quad top_;
+    Quad right_;
+    Quad bottom_;
+    Triangle front_;
+    Triangle back_;
+  };
+
+  /**
+   * Represents an axis-aligned bounding box (AABB) using two points. The min
+   * point is the bottom back left point, and the max point is the top front
+   * right point.
+   */
+  class AxisAlignedBox : public Shape {
+  public:
+    AxisAlignedBox();
+    AxisAlignedBox(const Vector& min, const Vector& max);
+
+    /** Returns the min point, x0. */
+    inline const Vector& min() const { return min_; }
+
+    /** Returns the max point, x7. */
+    inline const Vector& max() const { return max_; }
+
+    /** Returns the left bottom back point. */
+    inline const Vector& x0() const { return min_; }
+
+    /** Returns the right bottom back point. */
+    inline Vector x1() const { return Vector(max_.x, min_.y, min_.z); }
+
+    /** Returns the right bottom front point. */
+    inline Vector x2() const { return Vector(max_.x, min_.y, max_.z); }
+
+    /** Returns the left bottom front point. */
+    inline Vector x3() const { return Vector(min_.x, min_.y, max_.z); }
+
+    /** Returns the right top back point. */
+    inline Vector x4() const { return Vector(max_.x, max_.y, min_.z); }
+
+    /** Returns the left top back point. */
+    inline Vector x5() const { return Vector(min_.x, max_.y, min_.z); }
+
+    /** Returns the left top front point. */
+    inline Vector x6() const { return Vector(min_.x, max_.y, max_.z); }
+
+    /** Returns the right top front point. */
+    inline const Vector& x7() const { return max_; }
+
+    /** Returns true if this box contains the specified point. */
+    bool contains(const Vector& p) const;
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& p) const;
+
+  private:
+    Projection projectOut(const Vector& p) const;
+    Projection projectIn(const Vector& p) const;
+
+    Vector min_;
+    Vector max_;
+  };
+
+  /**
+   * Represents an oriented bounding box (OBB) using eight points.
+   */
+  class Box : public Shape {
+  public:
+    Box();
+    Box(const AxisAlignedBox& aab);
+    Box(const Vector& c, const Vector& x, const Vector& y, const Vector& z);
+    Box(const Vector& x0, const Vector& x1, const Vector& x2, const Vector& x3,
+        const Vector& x4, const Vector& x5, const Vector& x6, const Vector& x7);
+
+    /** Returns the left bottom back point. */
+    inline const Vector& x0() const { return bottom_.x0(); }
+
+    /** Returns the right bottom back point. */
+    inline const Vector& x1() const { return bottom_.x1(); }
+
+    /** Returns the right bottom front point. */
+    inline const Vector& x2() const { return bottom_.x2(); }
+
+    /** Returns the left bottom front point. */
+    inline const Vector& x3() const { return bottom_.x3(); }
+
+    /** Returns the right top back point. */
+    inline const Vector& x4() const { return top_.x0(); }
+
+    /** Returns the left top back point. */
+    inline const Vector& x5() const { return top_.x1(); }
+
+    /** Returns the left top front point. */
+    inline const Vector& x6() const { return top_.x2(); }
+
+    /** Returns the right top front point. */
+    inline const Vector& x7() const { return top_.x3(); }
+
+    /** Returns the left quad. */
+    inline const Quad& left() const { return back_; }
+
+    /** Returns the right quad. */
+    inline const Quad& right() const { return right_; }
+
+    /** Returns the bottom quad. */
+    inline const Quad& bottom() const { return bottom_; }
+
+    /** Returns the top quad. */
+    inline const Quad& top() const { return top_; }
+
+    /** Returns the back quad. */
+    inline const Quad& back() const { return back_; }
+
+    /** Returns the front quad. */
+    inline const Quad& front() const { return front_; }
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& p) const;
+
+  private:
+    Quad top_;
+    Quad bottom_;
+    Quad left_;
+    Quad right_;
+    Quad front_;
+    Quad back_;
+  };
+
+  /** Represents a cylinder as two points and a radius. */
+  class Cylinder : public Shape {
+  public:
+    Cylinder();
+    Cylinder(const Vector& x0, const Vector& x1, float radius);
+
+    /** Returns the start point of the cylinder's main axis. */
+    inline const Vector& x0() const { return axis_.x0(); }
+
+    /** Returns the end point of the cylinder's main axis. */
+    inline const Vector& x1() const { return axis_.x1(); }
+
+    /** Returns the radius. */
+    inline float radius() const { return r_; }
+
+    /** Returns the length of the cylinder. */
+    inline float length() const { return l_; }
+
+    /** Returns the unit direction vector of the cylinder. */
+    inline const Vector& z() const { return v_; }
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& p) const;
+
+  private:
+    LineSegment axis_;
+    float l_;
+    float r_;
+    Vector v_;
+  };
+
+};
+
+#endif

physics/shape_test.cpp

@@ -0,0 +1,301 @@
+// -*- C++ -*-
+
+#include <iostream>
+#include <math.h>
+#include <stdarg.h>
+#include <stdio.h>
+
+#include "shape.h"
+
+using namespace mbostock;
+
+static int returnCode = 0;
+
+void assertTrue(bool condition, char* message, ...) {
+  if (!condition) {
+    va_list args;
+    va_start(args, message);
+    printf("assertion failed: ");
+    vprintf(message, args);
+    printf("\n");
+    va_end(args);
+  }
+}
+
+static void testLineXYZ() {
+  printf("testLineXYZ...\n");
+  LineSegment x(Vector(0, 0, 0), Vector(2, 0, 0));
+  Projection px = x.project(Vector(1, 1, 0));
+  assertTrue(px.length == 1, "px.length");
+  assertTrue(px.x == Vector(1, 0, 0), "px.x");
+
+  LineSegment y(Vector(1, 1, 1), Vector(1, 4, 1));
+  Projection py = y.project(Vector(2, 1, 1));
+  assertTrue(py.length == 1, "py.length");
+  assertTrue(py.x == Vector(1, 1, 1), "py.x");
+
+  LineSegment z(Vector(-2, -2, -2), Vector(-2, -2, -4));
+  Projection pz = z.project(Vector(-2, -2, -2));
+  assertTrue(pz.length == 0, "pz.length");
+  assertTrue(pz.x == Vector(-2, -2, -2), "pz.x");
+}
+
+static void testLineX() {
+  printf("testLineX...\n");
+  LineSegment x(Vector(0, 0, 0), Vector(2, 0, 0));
+
+  /* Colinear, before x0. */
+  Projection p0 = x.project(Vector(-1, 0, 0));
+  assertTrue(p0.length == 1, "p0.length");
+  assertTrue(p0.x == Vector(0, 0, 0), "p0.x");
+
+  /* Colinear, after x1. */
+  Projection p1 = x.project(Vector(3, 0, 0));
+  assertTrue(p1.length == 1, "p1.length");
+  assertTrue(p1.x == Vector(2, 0, 0), "p1.x");
+
+  /* Colinear, at x0. */
+  Projection p2 = x.project(Vector(0, 0, 0));
+  assertTrue(p2.length == 0, "p2.length");
+  assertTrue(p2.x == Vector(0, 0, 0), "p2.x");
+
+  /* Colinear, at x1. */
+  Projection p3 = x.project(Vector(2, 0, 0));
+  assertTrue(p3.length == 0, "p3.length");
+  assertTrue(p3.x == Vector(2, 0, 0), "p3.x");
+
+  /* Colinear, between x0 and x1. */
+  for (float f = 0; f <= 2; f += .1f) {
+    Projection p4 = x.project(Vector(f, 0, 0));
+    assertTrue(p4.length == 0, "p4.length");
+    assertTrue(p4.x == Vector(f, 0, 0), "p4.x");
+  }
+
+  /* Noncolinear, before x0. */
+  Projection p5 = x.project(Vector(-1, -1, -1));
+  assertTrue(p5.length == sqrtf(3), "p5.length");
+  assertTrue(p5.x == Vector(0, 0, 0), "p5.x");
+
+  /* Noncolinear, after x1. */
+  Projection p6 = x.project(Vector(3, 1, 1));
+  assertTrue(p6.length == sqrtf(3), "p6.length");
+  assertTrue(p6.x == Vector(2, 0, 0), "p6.x");
+
+  /* Noncolinear, between x0 and x1. */
+  Projection p7 = x.project(Vector(1, 1, 1));
+  assertTrue(p7.length == sqrtf(2), "p7.length");
+  assertTrue(p7.x == Vector(1, 0, 0), "p7.x");
+}
+
+static void testPlaneXYZ() {
+  printf("testPlaneXYZ...\n");
+  Plane xz(Vector(0, 0, 0), Vector(0, 1, 0));
+  Projection pxz = xz.project(Vector(1, 2, 3));
+  assertTrue(pxz.length == 2, "pxz.length");
+  assertTrue(pxz.x == Vector(1, 0, 3), "pxz.x");
+
+  Plane xy(Vector(0, 0, 0), Vector(0, 0, 1));
+  Projection pxy = xy.project(Vector(1, 2, 3));
+  assertTrue(pxy.length == 3, "pxy.length");
+  assertTrue(pxy.x == Vector(1, 2, 0), "pxy.x");
+
+  Plane yz(Vector(0, 0, 0), Vector(1, 0, 0));
+  Projection pyz = yz.project(Vector(1, 2, 3));
+  assertTrue(pyz.length == 1, "pyz.length");
+  assertTrue(pyz.x == Vector(0, 2, 3), "pyz.x");
+
+  Projection pyz2 = yz.project(Vector(-1, 2, 3));
+  assertTrue(pyz2.length == 1, "pyz2.length");
+  assertTrue(pyz2.x == Vector(0, 2, 3), "pyz2.x");
+}
+
+static void testTriangle() {
+  printf("testTriangle...\n");
+
+  Triangle xy(Vector(1, 0, 0), Vector(0, 1, 0), Vector(0, 0, 0));
+  assertTrue(xy.normal() == Vector(0, 0, 1), "xy.normal");
+
+  /* At x0. */
+  Projection p0 = xy.project(Vector(0, 0, 0));
+  assertTrue(p0.length == 0, "p0.length");
+  assertTrue(p0.x == Vector(0, 0, 0), "p0.x");
+
+  /* At x1. */
+  Projection p1 = xy.project(Vector(0, 1, 0));
+  assertTrue(p1.length == 0, "p1.length");
+  assertTrue(p1.x == Vector(0, 1, 0), "p1.x");
+
+  /* At x2. */
+  Projection p2 = xy.project(Vector(1, 0, 0));
+  assertTrue(p2.length == 0, "p2.length");
+  assertTrue(p2.x == Vector(1, 0, 0), "p2.x");
+
+  /* In the plane of the triangle, x < x0. */
+  Projection p3 = xy.project(Vector(-1, 0, 0));
+  assertTrue(p3.length == 1, "p3.length");
+  assertTrue(p3.x == Vector(0, 0, 0), "p3.x");
+
+  /* In the plane of the triangle, x > x2. */
+  Projection p4 = xy.project(Vector(2, 0, 0));
+  assertTrue(p4.length == 1, "p4.length");
+  assertTrue(p4.x == Vector(1, 0, 0), "p4.x");
+
+  /* In the plane of the triangle, x = x2, y = x1. */
+  Projection p5 = xy.project(Vector(1, 1, 0));
+  assertTrue(p5.length == sqrtf(2) / 2, "p5.length");
+  assertTrue(p5.x == Vector(.5, .5, 0), "p5.x");
+
+  /* In front of the triangle (+z). */
+  Projection p6 = xy.project(Vector(.3, .3, 1));
+  assertTrue(p6.length == 1, "p6.length");
+  assertTrue(p6.x == Vector(.3, .3, 0), "p6.x");
+}
+
+static void testQuad() {
+  printf("testQuad...\n");
+
+  Quad xy(Vector(0, 0, 0), Vector(1, 0, 0), Vector(1, 1, 0), Vector(0, 1, 0));
+  assertTrue(xy.normal() == Vector(0, 0, 1), "xy.normal");
+
+  /* At x0. */
+  Projection p0 = xy.project(Vector(0, 0, 0));
+  assertTrue(p0.length == 0, "p0.length");
+  assertTrue(p0.x == Vector(0, 0, 0), "p0.x");
+
+  /* At x1. */
+  Projection p1 = xy.project(Vector(0, 1, 0));
+  assertTrue(p1.length == 0, "p1.length");
+  assertTrue(p1.x == Vector(0, 1, 0), "p1.x");
+
+  /* At x2. */
+  Projection p2 = xy.project(Vector(1, 0, 0));
+  assertTrue(p2.length == 0, "p2.length");
+  assertTrue(p2.x == Vector(1, 0, 0), "p2.x");
+
+  /* In the plane of the quad, x < x0. */
+  Projection p3 = xy.project(Vector(-1, 0, 0));
+  assertTrue(p3.length == 1, "p3.length");
+  assertTrue(p3.x == Vector(0, 0, 0), "p3.x");
+
+  /* In the plane of the quad, x > x2. */
+  Projection p4 = xy.project(Vector(2, 0, 0));
+  assertTrue(p4.length == 1, "p4.length");
+  assertTrue(p4.x == Vector(1, 0, 0), "p4.x");
+
+  /* In the plane of the quad, x = x2, y = x1. */
+  Projection p5 = xy.project(Vector(2, 2, 0));
+  assertTrue(p5.length == sqrtf(2), "p5.length");
+  assertTrue(p5.x == Vector(1, 1, 0), "p5.x");
+
+  /* In the plane of the quad. */
+  Projection p6 = xy.project(Vector(.5, .5, 0));
+  assertTrue(p6.length == 0, "p6.length");
+  assertTrue(p6.x == Vector(.5, .5, 0), "p6.x");
+
+  /* In front of the quad (+z). */
+  Projection p7 = xy.project(Vector(.3, .3, 1));
+  assertTrue(p7.length == 1, "p7.length");
+  assertTrue(p7.x == Vector(.3, .3, 0), "p6.x");
+}
+
+static void testRamp() {
+  printf("testRamp...\n");
+
+  Wedge r(Vector(-1, 0, .5),
+          Vector(-.5, .25, .5),
+          Vector(-.5, .25, -.5),
+          Vector(-1, 0, -.5));
+
+  Projection p0 = r.project(Vector(-0.694026, 0.05, 0.35));
+  assertTrue(p0.length > 0, "p0.length");
+}
+
+static void testCylinderIntersects() {
+  std::cout << "testCylinderIntersects...\n";
+  Cylinder c(Vector(0, 0, 0), Vector(0, 2, 0), 1);
+
+  Sphere s1(Vector(0, 3.1, 0), 1);
+  assertTrue(!c.intersects(s1), "c.intersects(s1)");
+
+  Sphere s2(Vector(0, 2.9, 0), 1);
+  assertTrue(c.intersects(s2), "c.intersects(s2)");
+
+  Sphere s3(Vector(0, 3, 0), .9);
+  assertTrue(!c.intersects(s3), "c.intersects(s3)");
+
+  Sphere s4(Vector(0, 3, 0), 1.1);
+  assertTrue(c.intersects(s4), "c.intersects(s4)");
+
+  Sphere s5(Vector(0, -1.1, 0), 1);
+  assertTrue(!c.intersects(s5), "c.intersects(s5)");
+
+  Sphere s6(Vector(0, -.9, 0), 1);
+  assertTrue(c.intersects(s6), "c.intersects(s6)");
+
+  Sphere s7(Vector(0, -1, 0), .9);
+  assertTrue(!c.intersects(s7), "c.intersects(s7)");
+
+  Sphere s8(Vector(0, -1, 0), 1.1);
+  assertTrue(c.intersects(s8), "c.intersects(s8)");
+
+  Sphere s9(Vector(2, 1, 0), .9);
+  assertTrue(!c.intersects(s9), "c.intersects(s9)");
+
+  Sphere s10(Vector(2, 1, 0), 1.1);
+  assertTrue(c.intersects(s10), "c.intersects(s10)");
+
+  Sphere s11(Vector(-2, 1, 0), .9);
+  assertTrue(!c.intersects(s11), "c.intersects(s11)");
+
+  Sphere s12(Vector(-2, 1, 0), 1.1);
+  assertTrue(c.intersects(s12), "c.intersects(s12)");
+
+  Sphere s13(Vector(0, 1, 2), .9);
+  assertTrue(!c.intersects(s13), "c.intersects(s13)");
+
+  Sphere s14(Vector(0, 1, 2), 1.1);
+  assertTrue(c.intersects(s14), "c.intersects(s14)");
+
+  Sphere s15(Vector(0, 1, -2), .9);
+  assertTrue(!c.intersects(s15), "c.intersects(s15)");
+
+  Sphere s16(Vector(0, 1, -2), 1.1);
+  assertTrue(c.intersects(s16), "c.intersects(s16)");
+}
+
+static void testCylinderProject() {
+  std::cout << "testCylinderProject...\n";
+  Cylinder c(Vector(0, 0, 0), Vector(0, 2, 0), 1);
+
+  Projection j1 = c.project(Vector(0, 3.1, 0));
+  assertTrue(Vector(0, 2, 0) == j1.x, "c.project(p1).x");
+  assertTrue(j1.length > 1.f && j1.length < 1.2f, "c.project(p1).length");
+  assertTrue(Vector(0, 1, 0) == j1.normal, "c.project(p1).normal");
+
+  Projection j2 = c.project(Vector(0, -1.1, 0));
+  assertTrue(Vector(0, 0, 0) == j2.x, "c.project(p2).x");
+  assertTrue(j2.length > 1.f && j2.length < 1.2f, "c.project(p2).length");
+  assertTrue(Vector(0, -1, 0) == j2.normal, "c.project(p2).normal");
+
+  Projection j3 = c.project(Vector(2.1, 1, 0));
+  assertTrue(Vector(1, 1, 0) == j3.x, "c.project(p3).x");
+  assertTrue(j3.length > 1.f && j3.length < 1.2f, "c.project(p3).length");
+  assertTrue(Vector(1, 0, 0) == j3.normal, "c.project(p3).normal");
+
+  Projection j4 = c.project(Vector(-2.1, 1, 0));
+  assertTrue(Vector(-1, 1, 0) == j4.x, "c.project(p4).x");
+  assertTrue(j4.length > 1.f && j4.length < 1.2f, "c.project(p4).length");
+  assertTrue(Vector(-1, 0, 0) == j4.normal, "c.project(p4).normal");
+}
+
+int main(int argc, char** argv) {
+  testLineXYZ();
+  testLineX();
+  testPlaneXYZ();
+  testTriangle();
+  testQuad();
+  testRamp();
+  testCylinderIntersects();
+  testCylinderProject();
+  return returnCode;
+}

physics/transform.cpp

@@ -0,0 +1,13 @@
+// -*- C++ -*-
+
+#include "transform.h"
+
+using namespace mbostock;
+
+Transform::Transform()
+    : enabled_(true) {
+}
+
+void Transform::enable(bool enabled) {
+  enabled_ = enabled;
+}

physics/transform.h

@@ -0,0 +1,25 @@
+// -*- C++ -*-
+
+#ifndef MBOSTOCK_TRANSFORM_H
+#define MBOSTOCK_TRANSFORM_H
+
+namespace mbostock {
+
+  class Transform {
+  public:
+    Transform();
+    virtual ~Transform() {}
+
+    void enable(bool enabled = true);
+    inline bool enabled() const { return enabled_; }
+
+    virtual void reset() = 0;
+    virtual void step() = 0;
+
+  private:
+    bool enabled_;
+  };
+
+}
+
+#endif

physics/translation.cpp

@@ -0,0 +1,87 @@
+// -*- C++ -*-
+
+#include "particle.h"
+#include "translation.h"
+#include "vector.h"
+
+using namespace mbostock;
+
+Translation::Translation(const Vector& x0, const Vector& x1,
+                         float speed, float start, float dampen)
+    : x0_(x0), x1_(x1), u_(start), s_(speed * ParticleSimulator::timeStep()),
+      kd_(1.f - dampen), v_((x1 - x0) * s_),
+      x_(x0 * (1.f - u_) + x1 * u_), origin_(x_),
+      mode_(REVERSE), reversed_(false) {
+}
+
+void Translation::reset() {
+  origin_ = x_ = x0_ * (1.f - u_) + x1_ * u_;
+  v_ = (x1_ - x0_) * s_;
+  reversed_ = false;
+}
+
+void Translation::step() {
+  if (!enabled()) {
+    return;
+  }
+  Vector x = origin_ + v_;
+  if (reversed_) {
+    if (v_.dot(x0_ - x) < 0.f) {
+      reversed_ = false;
+      v_ *= -1.f;
+    }
+  } else {
+    if (v_.dot(x1_ - x) < 0.f) {
+      switch (mode_) {
+        case REVERSE: {
+          reversed_ = true;
+          v_ *= -1.f;
+          break;
+        }
+        case RESET: {
+          x_ = x0_;
+          origin_ = x0_;
+          break;
+        }
+        case ONE_WAY: {
+          v_ = Vector::ZERO();
+          break;
+        }
+      }
+    }
+  }
+  x_ += v_;
+  dv_ = (x_ - origin_) * kd_;
+  origin_ += dv_;
+}
+
+void Translation::setMode(Mode m) {
+  mode_ = m;
+}
+
+const Vector& Translation::velocity() const {
+  return enabled() ? dv_ : Vector::ZERO();
+}
+
+Vector Translation::point(const Vector& x) const {
+  return x + origin_;
+}
+
+Vector Translation::pointInverse(const Vector& x) const {
+  return x - origin_;
+}
+
+TranslatingShape::TranslatingShape(const Shape& s, const Translation& t)
+    : shape_(s), translation_(t) {
+}
+
+bool TranslatingShape::intersects(const Sphere& s) const {
+  Sphere ts(translation_.pointInverse(s.x()), s.radius());
+  return shape_.intersects(ts);
+}
+
+Projection TranslatingShape::project(const Vector& x) const {
+  Projection p = shape_.project(translation_.pointInverse(x));
+  p.x = translation_.point(p.x);
+  return p;
+}

physics/translation.h

@@ -0,0 +1,70 @@
+// -*- C++ -*-
+
+#ifndef MBOSTOCK_TRANSLATION_H
+#define MBOSTOCK_TRANSLATION_H
+
+#include "shape.h"
+#include "transform.h"
+#include "vector.h"
+
+namespace mbostock {
+
+  class Translation : public Transform {
+  public:
+    Translation(const Vector& x0, const Vector& x1,
+                float s, float u, float kd);
+
+    enum Mode { REVERSE, RESET, ONE_WAY };
+
+    /* Sets the translation mode. The default is REVERSE. */
+    void setMode(Mode m);
+
+    /** Translates the specified point. */
+    Vector point(const Vector& x) const;
+
+    /** Inverse-translates the specified point. */
+    Vector pointInverse(const Vector& x) const;
+
+    /** Advances the translation by one time step. */
+    virtual void step();
+
+    /** Resets the translation. */
+    virtual void reset();
+
+    /** Returns the current velocity. */
+    const Vector& velocity() const;
+
+    /** Returns the current origin. */
+    inline const Vector& origin() const { return origin_; }
+
+  private:
+    void update();
+
+    Vector x0_;
+    Vector x1_;
+    float s_;
+    float u_;
+    float kd_;
+    Vector v_;
+    Vector dv_;
+    Vector x_;
+    Vector origin_;
+    Mode mode_;
+    bool reversed_;
+  };
+
+  class TranslatingShape : public Shape {
+  public:
+    TranslatingShape(const Shape& s, const Translation& t);
+
+    virtual bool intersects(const Sphere& s) const;
+    virtual Projection project(const Vector& x) const;
+
+  private:
+    const Shape& shape_;
+    const Translation& translation_;
+  };
+
+}
+
+#endif

physics/vector.cpp

@@ -0,0 +1,52 @@
+// -*- C++ -*-
+
+#include <algorithm>
+#include <stdlib.h>
+
+#include "vector.h"
+
+using namespace mbostock;
+
+const Vector& Vector::ZERO() {
+  static const Vector v;
+  return v;
+}
+
+const Vector& Vector::X() {
+  static const Vector v(1.f, 0.f, 0.f);
+  return v;
+}
+
+const Vector& Vector::Y() {
+  static const Vector v(0.f, 1.f, 0.f);
+  return v;
+}
+
+const Vector& Vector::Z() {
+  static const Vector v(0.f, 0.f, 1.f);
+  return v;
+}
+
+const Vector& Vector::INF() {
+  static const Vector v(INFINITY, INFINITY, INFINITY);
+  return v;
+}
+
+static float randomf() {
+  return random() / (float) RAND_MAX;
+}
+
+Vector Vector::min(const Vector& a, const Vector& b) {
+  return Vector(std::min(a.x, b.x), std::min(a.y, b.y), std::min(a.z, b.z));
+}
+
+Vector Vector::max(const Vector& a, const Vector& b) {
+  return Vector(std::max(a.x, b.x), std::max(a.y, b.y), std::max(a.z, b.z));
+}
+
+Vector Vector::randomVector(float k) {
+  Vector v(randomf() - .5f, randomf() - .5f, randomf() - .5f);
+  return ((v.x == 0.f) && (v.y == 0.f) && (v.z == 0.f))
+    ? Vector(k, 0.f, 0.f) // very unlikely, but still...
+    : v.normalize() * k;
+}

physics/vector.h

@@ -0,0 +1,134 @@
+// -*- C++ -*-
+
+#ifndef MBOSTOCK_VECTOR_H
+#define MBOSTOCK_VECTOR_H
+
+#include "math.h"
+
+namespace mbostock {
+
+  class Vector {
+  public:
+    inline Vector() : x(0.f), y(0.f), z(0.f) {}
+    inline Vector(float x, float y) : x(x), y(y), z(0.f) {}
+    inline Vector(float x, float y, float z) : x(x), y(y), z(z) {}
+
+    inline Vector operator*(float k) const {
+      return Vector(x * k, y * k, z * k);
+    }
+
+    inline Vector operator/(float k) const {
+      return Vector(x / k, y / k, z / k);
+    }
+
+    inline Vector operator-(float k) const {
+      return Vector(x - k, y - k, z - k);
+    }
+
+    inline Vector operator+(float k) const {
+      return Vector(x + k, y + k, z + k);
+    }
+
+    inline Vector operator-() const {
+      return Vector(-x, -y, -z);
+    }
+
+    inline Vector& operator*=(float k) {
+      x *= k; y *= k; z *= k;
+      return *this;
+    }
+    inline Vector& operator/=(float k) {
+      x /= k; y /= k; z /= k;
+      return *this;
+    }
+    inline Vector& operator-=(float k) {
+      x -= k; y -= k; z -= k;
+      return *this;
+    }
+    inline Vector& operator+=(float k) {
+      x += k; y += k; z += k;
+      return *this;
+    }
+
+    inline Vector operator-(const Vector& v) const {
+      return Vector(x - v.x, y - v.y, z - v.z);
+    }
+
+    inline Vector operator+(const Vector& v) const {
+      return Vector(x + v.x, y + v.y, z + v.z);
+    }
+
+    inline Vector operator*(const Vector& v) const {
+      return Vector(x * v.x, y * v.y, z * v.z);
+    }
+
+    inline Vector operator/(const Vector& v) const {
+      return Vector(x / v.x, y / v.y, z / v.z);
+    }
+
+    inline Vector& operator-=(const Vector& v) {
+      x -= v.x; y -= v.y; z -= v.z;
+      return *this;
+    }
+
+    inline Vector& operator+=(const Vector& v) {
+      x += v.x; y += v.y; z += v.z;
+      return *this;
+    }
+
+    inline Vector& operator*=(const Vector& v) {
+      x *= v.x; y *= v.y; z *= v.z;
+      return *this;
+    }
+
+    inline Vector& operator/=(const Vector& v) {
+      x /= v.x; y /= v.y; z /= v.z;
+      return *this;
+    }
+
+    inline bool operator==(const Vector& v) const {
+      return (x == v.x) && (y == v.y) && (z == v.z);
+    }
+
+    inline bool operator!=(const Vector& v) const {
+      return (x != v.x) || (y != v.y) || (z != v.z);
+    }
+
+    inline float dot(const Vector& v) const {
+      return x * v.x + y * v.y + z * v.z;
+    }
+
+    inline Vector cross(const Vector& v) const {
+      return Vector(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
+    }
+
+    inline float length() const {
+      return sqrtf(x * x + y * y + z * z);
+    }
+
+    inline float squared() const {
+      return x * x + y * y + z * z;
+    }
+
+    inline Vector normalize() const {
+      return *this / length();
+    }
+
+    static Vector min(const Vector& a, const Vector& b);
+    static Vector max(const Vector& a, const Vector& b);
+    static Vector randomVector(float k);
+
+    static const Vector& ZERO();
+    static const Vector& X();
+    static const Vector& Y();
+    static const Vector& Z();
+    static const Vector& INF();
+
+    float x;
+    float y;
+    float z;
+  };
+
+}
+
+#endif

physics/vector_test.cpp

@@ -0,0 +1,240 @@
+// -*- C++ -*-
+
+#include <stdio.h>
+#include <stdarg.h>
+
+#include "vector.h"
+
+using namespace mbostock;
+
+static int returnCode = 0;
+
+void assertTrue(bool condition, char* message, ...) {
+  if (!condition) {
+    va_list args;
+    va_start(args, message);
+    printf("assertion failed: ");
+    vprintf(message, args);
+    printf("\n");
+    va_end(args);
+  }
+}
+
+static void testDefaultConstructor() {
+  printf("testDefaultConstructor...\n");
+  Vector v1;
+  Vector v2;
+  assertTrue(v1 == v2, "v1 != v2");
+  assertTrue(v1.x == 0.f, "v1.x != 0.f");
+  assertTrue(v1.y == 0.f, "v1.y != 0.f");
+  assertTrue(v1.z == 0.f, "v1.z != 0.f");
+}
+
+static void testExplicitConstructor() {
+  printf("testExplicitConstructor...\n");
+  Vector v(1.f, 2.f, 3.f);
+  assertTrue(v.x == 1.f, "v.x != 1.f");
+  assertTrue(v.y == 2.f, "v.y != 2.f");
+  assertTrue(v.z == 3.f, "v.z != 3.f");
+}
+
+static void testAssignment() {
+  printf("testAssignment...\n");
+  Vector v1(1.f, 2.f, 3.f);
+  Vector v2(4.f, 5.f, 6.f);
+  Vector v3(4.f, 5.f, 6.f);
+  v1 = v2;
+  assertTrue(v1 == v2, "v1 != v2");
+  assertTrue(v1 == v3, "v1 != v3");
+  assertTrue(v2 == v3, "v2 != v3");
+}
+
+static void testCopyConstructor() {
+  printf("testCopyConstructor...\n");
+  Vector v1(1.f, 2.f, 3.f);
+  Vector v2(v1);
+  assertTrue(v1 == v2, "v1 != v2");
+  assertTrue(v2 == v1, "v2 != v1");
+}
+
+static void testEqualVector() {
+  printf("testEqualVector...\n");
+  Vector v1a(1.f, 2.f, 3.f);
+  Vector v1b(1.f, 2.f, 3.f);
+  Vector v2(1.f, 2.f, 4.f);
+  Vector v3(1.f, 3.f, 3.f);
+  Vector v4(2.f, 3.f, 3.f);
+  assertTrue(v1a == v1b, "v1a != v1b");
+  assertTrue(v1b == v1a, "v1b != v1a");
+  assertTrue(!(v1a == v2), "v1a == v2");
+  assertTrue(!(v1a == v3), "v1a == v3");
+  assertTrue(!(v1a == v4), "v1a == v4");
+}
+
+static void testNotEqualVector() {
+  printf("testNotEqualVector...\n");
+  Vector v1a(1.f, 2.f, 3.f);
+  Vector v1b(1.f, 2.f, 3.f);
+  Vector v2(1.f, 2.f, 4.f);
+  Vector v3(1.f, 3.f, 3.f);
+  Vector v4(2.f, 3.f, 3.f);
+  assertTrue(!(v1a != v1b), "v1a != v1b");
+  assertTrue(!(v1b != v1a), "v1b != v1a");
+  assertTrue(v1a != v2, "v1a == v2");
+  assertTrue(v1a != v3, "v1a == v3");
+  assertTrue(v1a != v4, "v1a == v4");
+}
+
+static void testMultiplyScalar() {
+  printf("testMultiplyScalar...\n");
+  Vector v1(1.f, 2.f, 3.f);
+  Vector v2(2.f, 4.f, 6.f);
+  assertTrue(v2 == (v1 * 2), "v2 != v1 * 2");
+  assertTrue(v1 == (v2 * .5), "v1 != v2 * .5");
+}
+
+static void testDivideScalar() {
+  printf("testDivideScalar...\n");
+  Vector v1(1.f, 2.f, 3.f);
+  Vector v2(2.f, 4.f, 6.f);
+  assertTrue(v2 == (v1 / .5), "v2 != v1 / .5");
+  assertTrue(v1 == (v2 / 2), "v1 != v2 / 2");
+}
+
+static void testAddScalar() {
+  printf("testAddScalar...\n");
+  Vector v1(1, 2, 3);
+  assertTrue(Vector(2, 3, 4) == (v1 + 1), "v1 + 1");
+  assertTrue(Vector(0, 1, 2) == (v1 + -1), "v1 + -1");
+}
+
+static void testSubtractScalar() {
+  printf("testSubtractScalar...\n");
+  Vector v1(1, 2, 3);
+  assertTrue(Vector(0, 1, 2) == (v1 - 1), "v1 - 1");
+  assertTrue(Vector(2, 3, 4) == (v1 - -1), "v1 - -1");
+}
+
+static void testMultiplyAssignmentScalar() {
+  printf("testMultiplyAssignmentScalar...\n");
+  Vector v1(1, 2, 3);
+  v1 *= 2;
+  assertTrue(Vector(2, 4, 6) == v1, "v1 * 2");
+  v1 *= .5;
+  assertTrue(Vector(1, 2, 3) == v1, "v1 * .5");
+}
+
+static void testDivideAssignmentScalar() {
+  printf("testDivideAssignmentScalar...\n");
+  Vector v1(1, 2, 3);
+  v1 /= .5;
+  assertTrue(Vector(2, 4, 6) == v1, "v1 / .5");
+  v1 /= 2;
+  assertTrue(Vector(1, 2, 3) == v1, "v1 / 2");
+}
+
+static void testAddAssignmentScalar() {
+  printf("testAddAssignmentScalar...\n");
+  Vector v1(1, 2, 3);
+  v1 += 1;
+  assertTrue(Vector(2, 3, 4) == v1, "v1 + 1");
+  v1 += -1;
+  assertTrue(Vector(1, 2, 3) == v1, "v1 + -1");
+}
+
+static void testSubtractAssignmentScalar() {
+  printf("testSubtractAssignmentScalar...\n");
+  Vector v1(1, 2, 3);
+  v1 -= -1;
+  assertTrue(Vector(2, 3, 4) == v1, "v1 - -1");
+  v1 -= 1;
+  assertTrue(Vector(1, 2, 3) == v1, "v1 - 1");
+}
+
+static void testAddVector() {
+  printf("testAddVector...\n");
+  Vector v1(1, 2, 3);
+  Vector v2 = v1 * 2;
+  Vector v3 = v1 + v2;
+  assertTrue(Vector(3, 6, 9) == v3, "v1 + v1 * 2");
+}
+
+static void testSubtractVector() {
+  printf("testSubtractVector...\n");
+  Vector v1(1, 2, 3);
+  Vector v2 = v1 * 2;
+  Vector v3 = v1 - v2;
+  assertTrue(Vector(-1, -2, -3) == v3, "v1 - v1 * 2");
+}
+
+static void testAddAssignmentVector() {
+  printf("testAddAssignmentVector...\n");
+  Vector v1(1, 2, 3);
+  v1 += v1 * 2;
+  assertTrue(Vector(3, 6, 9) == v1, "v1 + v1 * 2");
+}
+
+static void testSubtractAssignmentVector() {
+  printf("testSubtractAssignmentVector...\n");
+  Vector v1(1, 2, 3);
+  v1 -= v1 * 2;
+  assertTrue(Vector(-1, -2, -3) == v1, "v1 - v1 * 2");
+}
+
+static void testNegative() {
+  printf("testNegative...\n");
+  Vector v1(1, 2, 3);
+  Vector v2(-1, -2, -3);
+  assertTrue(v2 == -v1, "v2 != -v1");
+}
+
+static void testDotVector() {
+  printf("testDotVector...\n");
+  Vector v1(1, 2, 3);
+  Vector v2(-2, -3, -4);
+  float d = 1 * -2 + 2 * -3 + 3 * -4;
+  assertTrue(v1.dot(v2) == d, "v1 dot v2");
+}
+
+static void testCrossVector() {
+  printf("testCrossVector...\n");
+  Vector x(1, 0, 0);
+  Vector y(0, 1, 0);
+  Vector z(0, 0, 1);
+  assertTrue(x.cross(y) == z, "x cross y");
+}
+
+static void testLength() {
+  printf("testLength...\n");
+}
+
+static void testNormalize() {
+  printf("testNormalize...\n");
+}
+
+int main(int argc, char** argv) {
+  testDefaultConstructor();
+  testExplicitConstructor();
+  testAssignment();
+  testCopyConstructor();
+  testEqualVector();
+  testNotEqualVector();
+  testMultiplyScalar();
+  testDivideScalar();
+  testAddScalar();
+  testSubtractScalar();
+  testMultiplyAssignmentScalar();
+  testDivideAssignmentScalar();
+  testAddAssignmentScalar();
+  testSubtractAssignmentScalar();
+  testAddVector();
+  testSubtractVector();
+  testAddAssignmentVector();
+  testSubtractAssignmentVector();
+  testNegative();
+  testDotVector();
+  testCrossVector();
+  testLength();
+  testNormalize();
+  return returnCode;
+}