# HG changeset patch # User John Tsiombikas # Date 1423901280 -7200 # Node ID af24cfbdf9b69e802fb1cea2033c2067d1b65450 # Parent 2f872a1799144884eaaa28de435fd7336692b581 adding collision detection diff -r 2f872a179914 -r af24cfbdf9b6 src/co_collider.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/co_collider.h Sat Feb 14 10:08:00 2015 +0200 @@ -0,0 +1,12 @@ +#ifndef CO_COLLIDER_H_ +#define CO_COLLIDER_H_ + +#include "comp.h" +#include "geom.h" + +class CoCollider { +public: + GeomShape *shape; +}; + +#endif // CO_COLLIDER_H_ diff -r 2f872a179914 -r af24cfbdf9b6 src/co_dbgvis.h --- a/src/co_dbgvis.h Sat Feb 14 07:27:12 2015 +0200 +++ b/src/co_dbgvis.h Sat Feb 14 10:08:00 2015 +0200 @@ -1,7 +1,6 @@ #ifndef CO_DBGVIS_H_ #define CO_DBGVIS_H_ -#include "gobj.h" #include "comp.h" #include "co_xform.h" diff -r 2f872a179914 -r af24cfbdf9b6 src/geom.cc --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/geom.cc Sat Feb 14 10:08:00 2015 +0200 @@ -0,0 +1,240 @@ +#include +#include +#include "geom.h" + +static bool col_sphere_sphere(const Sphere *a, const Sphere *b, HitPoint *hit); +static bool col_plane_plane(const Plane *a, const Plane *b, HitPoint *hit); +static bool col_sphere_plane(const Sphere *sph, const Plane *plane, HitPoint *hit); + +GeomShape::GeomShape() +{ + type = GEOM_UNKNOWN; +} + +GeomShape::~GeomShape() +{ +} + +GeomShape::Type GeomShape::get_type() const +{ + return type; +} + +// ---- Sphere class ---- + +Sphere::Sphere() +{ + type = GEOM_SPHERE; + radius = 1.0f; +} + +Sphere::Sphere(const Vector3 &c, float rad) + : center(c) +{ + type = GEOM_SPHERE; + radius = rad; +} + +bool Sphere::intersect(const Ray &ray, HitPoint *hit) const +{ + float a = dot_product(ray.dir, ray.dir); + float b = 2.0 * ray.dir.x * (ray.origin.x - center.x) + + 2.0 * ray.dir.y * (ray.origin.y - center.y) + + 2.0 * ray.dir.z * (ray.origin.z - center.z); + float c = dot_product(ray.origin, ray.origin) + dot_product(center, center) - + 2.0 * dot_product(ray.origin, center) - radius * radius; + + float discr = b * b - 4.0 * a * c; + if(discr < 1e-4) { + return false; + } + + float sqrt_discr = sqrt(discr); + float t0 = (-b + sqrt_discr) / (2.0 * a); + float t1 = (-b - sqrt_discr) / (2.0 * a); + + if(t0 < 1e-4) + t0 = t1; + if(t1 < 1e-4) + t1 = t0; + + float t = t0 < t1 ? t0 : t1; + if(t < 1e-4) { + return false; + } + + // fill the HitPoint structure + if(hit) { + hit->shape = this; + hit->data = 0; + hit->dist = t; + hit->pos = ray.origin + ray.dir * t; + hit->normal = (hit->pos - center) / radius; + } + return true; +} + +bool Sphere::collide(const GeomShape *geom, HitPoint *hit) const +{ + switch(geom->get_type()) { + case GEOM_SPHERE: + return col_sphere_sphere(this, (const Sphere*)geom, hit); + case GEOM_PLANE: + return col_sphere_plane(this, (const Plane*)geom, hit); + default: + break; + } + return false; +} + +float Sphere::distance(const Vector3 &pt) const +{ + return sqrt(distance_sq(pt)); +} + +float Sphere::distance_sq(const Vector3 &pt) const +{ + return (pt - center).length_sq(); +} + +// ---- Plane class ---- + +Plane::Plane() + : normal(0, 1, 0) +{ + type = GEOM_PLANE; + dist = 0.0f; +} + +Plane::Plane(const Vector3 &n, float d) + : normal(n) +{ + type = GEOM_PLANE; + dist = d; + + normal.normalize(); +} + +Plane::Plane(float a, float b, float c, float d) + : normal(a, b, c) +{ + type = GEOM_PLANE; + dist = d; + + normal.normalize(); +} + +Plane::Plane(const Vector3 &pos, const Vector3 &norm) + : normal(norm) +{ + type = GEOM_PLANE; + + dist = 0.0f; + dist = distance(pos); +} + +Plane::Plane(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3) +{ + type = GEOM_PLANE; + + normal = cross_product(p2 - p1, p3 - p1).normalized(); + dist = 0.0f; + dist = distance(p1); +} + + +bool Plane::intersect(const Ray &ray, HitPoint *hit) const +{ + Vector3 pt = Vector3(0, 0, 0) + normal * dist; + + float ndotdir = dot_product(normal, ray.dir); + if(fabs(ndotdir) < 1e-4) { + return false; + } + + if(hit) { + Vector3 ptdir = pt - ray.origin; + float t = dot_product(normal, ptdir) / ndotdir; + + hit->pos = ray.origin + ray.dir * t; + hit->normal = normal; + hit->shape = this; + } + return true; +} + +bool Plane::collide(const GeomShape *geom, HitPoint *hit) const +{ + switch(geom->get_type()) { + case GEOM_SPHERE: + { + bool res = col_sphere_plane((const Sphere*)geom, this, hit); + if(hit) { + hit->normal = -hit->normal; + } + return res; + } + + case GEOM_PLANE: + return col_plane_plane(this, (const Plane*)geom, hit); + + default: + break; + } + return false; +} + +float Plane::distance(const Vector3 &v) const +{ + Vector3 pt = normal * dist; + return dot_product(v - pt, normal); +} + +float Plane::distance_sq(const Vector3 &v) const +{ + float d = distance(v); + return d * d; +} + + +// collision detection functions + +static bool col_sphere_sphere(const Sphere *a, const Sphere *b, HitPoint *hit) +{ + float sum_rad = a->radius + b->radius; + Vector3 dir = b->center - a->center; + float dist_sq = dir.length_sq(); + bool res = dist_sq <= sum_rad * sum_rad; + + if(res && hit) { + hit->pos = a->center + dir * 0.5; + hit->shape = b; + hit->normal = -dir; + } + return res; +} + +static bool col_plane_plane(const Plane *a, const Plane *b, HitPoint *hit) +{ + fprintf(stderr, "%s: not implemented\n", __FUNCTION__); + return false; +} + +static bool col_sphere_plane(const Sphere *sph, const Plane *plane, HitPoint *hit) +{ + float dist = plane->distance(sph->center); + bool res = fabs(dist) <= sph->radius; + + if(res && hit) { + Vector3 planept = plane->normal * plane->dist; + Vector3 sphdir = sph->center - planept; + if(dot_product(sphdir, plane->normal) >= 0.0f) { + hit->normal = plane->normal; + } else { + hit->normal = -plane->normal; + } + hit->pos = sph->center - plane->normal * fabs(dist); + hit->shape = plane; + } + return res; +} diff -r 2f872a179914 -r af24cfbdf9b6 src/geom.h --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/src/geom.h Sat Feb 14 10:08:00 2015 +0200 @@ -0,0 +1,66 @@ +#ifndef GEOM_H_ +#define GEOM_H_ + +#include + +class GeomShape; + +struct HitPoint { + float dist; + Vector3 pos; + Vector3 normal; + const GeomShape *shape; + void *data; +}; + +class GeomShape { +protected: + enum Type { GEOM_UNKNOWN, GEOM_SPHERE, GEOM_PLANE } type; + +public: + GeomShape(); + virtual ~GeomShape(); + + virtual Type get_type() const; + + virtual bool intersect(const Ray &ray, HitPoint *hit = 0) const = 0; + virtual bool collide(const GeomShape *geom, HitPoint *hit = 0) const = 0; + + virtual float distance(const Vector3 &pt) const = 0; + virtual float distance_sq(const Vector3 &pt) const = 0; +}; + +class Sphere : public GeomShape { +public: + Vector3 center; + float radius; + + Sphere(); + Sphere(const Vector3 &c, float rad); + + bool intersect(const Ray &ray, HitPoint *hit = 0) const; + bool collide(const GeomShape *geom, HitPoint *hit = 0) const; + + float distance(const Vector3 &pt) const; + float distance_sq(const Vector3 &pt) const; +}; + +class Plane : public GeomShape { +public: + Vector3 normal; + float dist; + + Plane(); + Plane(const Vector3 &n, float d); + Plane(float a, float b, float c, float d); + Plane(const Vector3 &pos, const Vector3 &norm); + Plane(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3); + + bool intersect(const Ray &ray, HitPoint *hit = 0) const; + bool collide(const GeomShape *geom, HitPoint *hit = 0) const; + + float distance(const Vector3 &pt) const; + float distance_sq(const Vector3 &pt) const; +}; + +#endif // GEOM_H_ diff -r 2f872a179914 -r af24cfbdf9b6 src/gobj.cc --- a/src/gobj.cc Sat Feb 14 07:27:12 2015 +0200 +++ b/src/gobj.cc Sat Feb 14 10:08:00 2015 +0200 @@ -78,9 +78,7 @@ sorted = true; } - printf("obj(%p) update\n", (void*)this); for(size_t i=0; iget_name()); comp[i]->update(dt); } } diff -r 2f872a179914 -r af24cfbdf9b6 src/sim.cc --- a/src/sim.cc Sat Feb 14 07:27:12 2015 +0200 +++ b/src/sim.cc Sat Feb 14 10:08:00 2015 +0200 @@ -9,7 +9,7 @@ void SimWorld::add_object(GObject *obj) { - CoRigid *co = COCAST(CoRigid, obj->get_component("phys")); + CoRigid *co = COCAST(CoRigid, obj->get_component("rigid")); if(co) { add_rigid_body(co); } else { @@ -27,7 +27,7 @@ void SimWorld::remove_object(GObject *obj) { - CoRigid *co = COCAST(CoRigid, obj->get_component("phys")); + CoRigid *co = COCAST(CoRigid, obj->get_component("rigid")); if(co) { remove_rigid_body(co); } else { diff -r 2f872a179914 -r af24cfbdf9b6 src/test.cc --- a/src/test.cc Sat Feb 14 07:27:12 2015 +0200 +++ b/src/test.cc Sat Feb 14 10:08:00 2015 +0200 @@ -37,7 +37,7 @@ glutCreateWindow("component system test"); glutDisplayFunc(display); - //glutIdleFunc(idle); + glutIdleFunc(idle); glutReshapeFunc(reshape); glutKeyboardFunc(keyb); glutMouseFunc(mouse); @@ -67,8 +67,9 @@ obj->add_component(new CoXForm); obj->add_component(new CoPRS); obj->add_component(new CoRigid); - COCAST(CoPRS, obj->get_component("prs"))->pos = Vector3(5, 1, 0); + COCAST(CoPRS, obj->get_component("prs"))->pos = Vector3(5, 2, 0); objects.push_back(obj); + simworld.add_object(obj); obj = new GObject; obj->add_component(new CoSphereVis);