Initial commit.
This commit is contained in:
Mike Bostock
commit
44b5f2392f
98 changed files with 11750 additions and 0 deletions
514
physics/shape.cpp
Normal file
514
physics/shape.cpp
Normal file
|
|
@ -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);
|
||||
}
|
||||
Loading…
Add table
Add a link
Reference in a new issue