coeng

changeset 7:af24cfbdf9b6

adding collision detection
author John Tsiombikas <nuclear@member.fsf.org>
date Sat, 14 Feb 2015 10:08:00 +0200
parents 2f872a179914
children 8cce82794f90
files src/co_collider.h src/co_dbgvis.h src/geom.cc src/geom.h src/gobj.cc src/sim.cc src/test.cc
diffstat 7 files changed, 323 insertions(+), 7 deletions(-) [+]
line diff
     1.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     1.2 +++ b/src/co_collider.h	Sat Feb 14 10:08:00 2015 +0200
     1.3 @@ -0,0 +1,12 @@
     1.4 +#ifndef CO_COLLIDER_H_
     1.5 +#define CO_COLLIDER_H_
     1.6 +
     1.7 +#include "comp.h"
     1.8 +#include "geom.h"
     1.9 +
    1.10 +class CoCollider {
    1.11 +public:
    1.12 +	GeomShape *shape;
    1.13 +};
    1.14 +
    1.15 +#endif	// CO_COLLIDER_H_
     2.1 --- a/src/co_dbgvis.h	Sat Feb 14 07:27:12 2015 +0200
     2.2 +++ b/src/co_dbgvis.h	Sat Feb 14 10:08:00 2015 +0200
     2.3 @@ -1,7 +1,6 @@
     2.4  #ifndef CO_DBGVIS_H_
     2.5  #define CO_DBGVIS_H_
     2.6  
     2.7 -#include "gobj.h"
     2.8  #include "comp.h"
     2.9  #include "co_xform.h"
    2.10  
     3.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     3.2 +++ b/src/geom.cc	Sat Feb 14 10:08:00 2015 +0200
     3.3 @@ -0,0 +1,240 @@
     3.4 +#include <float.h>
     3.5 +#include <algorithm>
     3.6 +#include "geom.h"
     3.7 +
     3.8 +static bool col_sphere_sphere(const Sphere *a, const Sphere *b, HitPoint *hit);
     3.9 +static bool col_plane_plane(const Plane *a, const Plane *b, HitPoint *hit);
    3.10 +static bool col_sphere_plane(const Sphere *sph, const Plane *plane, HitPoint *hit);
    3.11 +
    3.12 +GeomShape::GeomShape()
    3.13 +{
    3.14 +	type = GEOM_UNKNOWN;
    3.15 +}
    3.16 +
    3.17 +GeomShape::~GeomShape()
    3.18 +{
    3.19 +}
    3.20 +
    3.21 +GeomShape::Type GeomShape::get_type() const
    3.22 +{
    3.23 +	return type;
    3.24 +}
    3.25 +
    3.26 +// ---- Sphere class ----
    3.27 +
    3.28 +Sphere::Sphere()
    3.29 +{
    3.30 +	type = GEOM_SPHERE;
    3.31 +	radius = 1.0f;
    3.32 +}
    3.33 +
    3.34 +Sphere::Sphere(const Vector3 &c, float rad)
    3.35 +	: center(c)
    3.36 +{
    3.37 +	type = GEOM_SPHERE;
    3.38 +	radius = rad;
    3.39 +}
    3.40 +
    3.41 +bool Sphere::intersect(const Ray &ray, HitPoint *hit) const
    3.42 +{
    3.43 +	float a = dot_product(ray.dir, ray.dir);
    3.44 +	float b = 2.0 * ray.dir.x * (ray.origin.x - center.x) +
    3.45 +		2.0 * ray.dir.y * (ray.origin.y - center.y) +
    3.46 +		2.0 * ray.dir.z * (ray.origin.z - center.z);
    3.47 +	float c = dot_product(ray.origin, ray.origin) + dot_product(center, center) -
    3.48 +		2.0 * dot_product(ray.origin, center) - radius * radius;
    3.49 +
    3.50 +	float discr = b * b - 4.0 * a * c;
    3.51 +	if(discr < 1e-4) {
    3.52 +		return false;
    3.53 +	}
    3.54 +
    3.55 +	float sqrt_discr = sqrt(discr);
    3.56 +	float t0 = (-b + sqrt_discr) / (2.0 * a);
    3.57 +	float t1 = (-b - sqrt_discr) / (2.0 * a);
    3.58 +
    3.59 +	if(t0 < 1e-4)
    3.60 +		t0 = t1;
    3.61 +	if(t1 < 1e-4)
    3.62 +		t1 = t0;
    3.63 +
    3.64 +	float t = t0 < t1 ? t0 : t1;
    3.65 +	if(t < 1e-4) {
    3.66 +		return false;
    3.67 +	}
    3.68 +
    3.69 +	// fill the HitPoint structure
    3.70 +	if(hit) {
    3.71 +		hit->shape = this;
    3.72 +		hit->data = 0;
    3.73 +		hit->dist = t;
    3.74 +		hit->pos = ray.origin + ray.dir * t;
    3.75 +		hit->normal = (hit->pos - center) / radius;
    3.76 +	}
    3.77 +	return true;
    3.78 +}
    3.79 +
    3.80 +bool Sphere::collide(const GeomShape *geom, HitPoint *hit) const
    3.81 +{
    3.82 +	switch(geom->get_type()) {
    3.83 +	case GEOM_SPHERE:
    3.84 +		return col_sphere_sphere(this, (const Sphere*)geom, hit);
    3.85 +	case GEOM_PLANE:
    3.86 +		return col_sphere_plane(this, (const Plane*)geom, hit);
    3.87 +	default:
    3.88 +		break;
    3.89 +	}
    3.90 +	return false;
    3.91 +}
    3.92 +
    3.93 +float Sphere::distance(const Vector3 &pt) const
    3.94 +{
    3.95 +	return sqrt(distance_sq(pt));
    3.96 +}
    3.97 +
    3.98 +float Sphere::distance_sq(const Vector3 &pt) const
    3.99 +{
   3.100 +	return (pt - center).length_sq();
   3.101 +}
   3.102 +
   3.103 +// ---- Plane class ----
   3.104 +
   3.105 +Plane::Plane()
   3.106 +	: normal(0, 1, 0)
   3.107 +{
   3.108 +	type = GEOM_PLANE;
   3.109 +	dist = 0.0f;
   3.110 +}
   3.111 +
   3.112 +Plane::Plane(const Vector3 &n, float d)
   3.113 +	: normal(n)
   3.114 +{
   3.115 +	type = GEOM_PLANE;
   3.116 +	dist = d;
   3.117 +
   3.118 +	normal.normalize();
   3.119 +}
   3.120 +
   3.121 +Plane::Plane(float a, float b, float c, float d)
   3.122 +	: normal(a, b, c)
   3.123 +{
   3.124 +	type = GEOM_PLANE;
   3.125 +	dist = d;
   3.126 +
   3.127 +	normal.normalize();
   3.128 +}
   3.129 +
   3.130 +Plane::Plane(const Vector3 &pos, const Vector3 &norm)
   3.131 +	: normal(norm)
   3.132 +{
   3.133 +	type = GEOM_PLANE;
   3.134 +
   3.135 +	dist = 0.0f;
   3.136 +	dist = distance(pos);
   3.137 +}
   3.138 +
   3.139 +Plane::Plane(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3)
   3.140 +{
   3.141 +	type = GEOM_PLANE;
   3.142 +
   3.143 +	normal = cross_product(p2 - p1, p3 - p1).normalized();
   3.144 +	dist = 0.0f;
   3.145 +	dist = distance(p1);
   3.146 +}
   3.147 +
   3.148 +
   3.149 +bool Plane::intersect(const Ray &ray, HitPoint *hit) const
   3.150 +{
   3.151 +	Vector3 pt = Vector3(0, 0, 0) + normal * dist;
   3.152 +
   3.153 +	float ndotdir = dot_product(normal, ray.dir);
   3.154 +	if(fabs(ndotdir) < 1e-4) {
   3.155 +		return false;
   3.156 +	}
   3.157 +
   3.158 +	if(hit) {
   3.159 +		Vector3 ptdir = pt - ray.origin;
   3.160 +		float t = dot_product(normal, ptdir) / ndotdir;
   3.161 +
   3.162 +		hit->pos = ray.origin + ray.dir * t;
   3.163 +		hit->normal = normal;
   3.164 +		hit->shape = this;
   3.165 +	}
   3.166 +	return true;
   3.167 +}
   3.168 +
   3.169 +bool Plane::collide(const GeomShape *geom, HitPoint *hit) const
   3.170 +{
   3.171 +	switch(geom->get_type()) {
   3.172 +	case GEOM_SPHERE:
   3.173 +		{
   3.174 +			bool res = col_sphere_plane((const Sphere*)geom, this, hit);
   3.175 +			if(hit) {
   3.176 +				hit->normal = -hit->normal;
   3.177 +			}
   3.178 +			return res;
   3.179 +		}
   3.180 +
   3.181 +	case GEOM_PLANE:
   3.182 +		return col_plane_plane(this, (const Plane*)geom, hit);
   3.183 +
   3.184 +	default:
   3.185 +		break;
   3.186 +	}
   3.187 +	return false;
   3.188 +}
   3.189 +
   3.190 +float Plane::distance(const Vector3 &v) const
   3.191 +{
   3.192 +	Vector3 pt = normal * dist;
   3.193 +	return dot_product(v - pt, normal);
   3.194 +}
   3.195 +
   3.196 +float Plane::distance_sq(const Vector3 &v) const
   3.197 +{
   3.198 +	float d = distance(v);
   3.199 +	return d * d;
   3.200 +}
   3.201 +
   3.202 +
   3.203 +// collision detection functions
   3.204 +
   3.205 +static bool col_sphere_sphere(const Sphere *a, const Sphere *b, HitPoint *hit)
   3.206 +{
   3.207 +	float sum_rad = a->radius + b->radius;
   3.208 +	Vector3 dir = b->center - a->center;
   3.209 +	float dist_sq = dir.length_sq();
   3.210 +	bool res = dist_sq <= sum_rad * sum_rad;
   3.211 +
   3.212 +	if(res && hit) {
   3.213 +		hit->pos = a->center + dir * 0.5;
   3.214 +		hit->shape = b;
   3.215 +		hit->normal = -dir;
   3.216 +	}
   3.217 +	return res;
   3.218 +}
   3.219 +
   3.220 +static bool col_plane_plane(const Plane *a, const Plane *b, HitPoint *hit)
   3.221 +{
   3.222 +	fprintf(stderr, "%s: not implemented\n", __FUNCTION__);
   3.223 +	return false;
   3.224 +}
   3.225 +
   3.226 +static bool col_sphere_plane(const Sphere *sph, const Plane *plane, HitPoint *hit)
   3.227 +{
   3.228 +	float dist = plane->distance(sph->center);
   3.229 +	bool res = fabs(dist) <= sph->radius;
   3.230 +
   3.231 +	if(res && hit) {
   3.232 +		Vector3 planept = plane->normal * plane->dist;
   3.233 +		Vector3 sphdir = sph->center - planept;
   3.234 +		if(dot_product(sphdir, plane->normal) >= 0.0f) {
   3.235 +			hit->normal = plane->normal;
   3.236 +		} else {
   3.237 +			hit->normal = -plane->normal;
   3.238 +		}
   3.239 +		hit->pos = sph->center - plane->normal * fabs(dist);
   3.240 +		hit->shape = plane;
   3.241 +	}
   3.242 +	return res;
   3.243 +}
     4.1 --- /dev/null	Thu Jan 01 00:00:00 1970 +0000
     4.2 +++ b/src/geom.h	Sat Feb 14 10:08:00 2015 +0200
     4.3 @@ -0,0 +1,66 @@
     4.4 +#ifndef GEOM_H_
     4.5 +#define GEOM_H_
     4.6 +
     4.7 +#include <vmath/vmath.h>
     4.8 +
     4.9 +class GeomShape;
    4.10 +
    4.11 +struct HitPoint {
    4.12 +	float dist;
    4.13 +	Vector3 pos;
    4.14 +	Vector3 normal;
    4.15 +	const GeomShape *shape;
    4.16 +	void *data;
    4.17 +};
    4.18 +
    4.19 +class GeomShape {
    4.20 +protected:
    4.21 +	enum Type { GEOM_UNKNOWN, GEOM_SPHERE, GEOM_PLANE } type;
    4.22 +
    4.23 +public:
    4.24 +	GeomShape();
    4.25 +	virtual ~GeomShape();
    4.26 +
    4.27 +	virtual Type get_type() const;
    4.28 +
    4.29 +	virtual bool intersect(const Ray &ray, HitPoint *hit = 0) const = 0;
    4.30 +	virtual bool collide(const GeomShape *geom, HitPoint *hit = 0) const = 0;
    4.31 +
    4.32 +	virtual float distance(const Vector3 &pt) const = 0;
    4.33 +	virtual float distance_sq(const Vector3 &pt) const = 0;
    4.34 +};
    4.35 +
    4.36 +class Sphere : public GeomShape {
    4.37 +public:
    4.38 +	Vector3 center;
    4.39 +	float radius;
    4.40 +
    4.41 +	Sphere();
    4.42 +	Sphere(const Vector3 &c, float rad);
    4.43 +
    4.44 +	bool intersect(const Ray &ray, HitPoint *hit = 0) const;
    4.45 +	bool collide(const GeomShape *geom, HitPoint *hit = 0) const;
    4.46 +
    4.47 +	float distance(const Vector3 &pt) const;
    4.48 +	float distance_sq(const Vector3 &pt) const;
    4.49 +};
    4.50 +
    4.51 +class Plane : public GeomShape {
    4.52 +public:
    4.53 +	Vector3 normal;
    4.54 +	float dist;
    4.55 +
    4.56 +	Plane();
    4.57 +	Plane(const Vector3 &n, float d);
    4.58 +	Plane(float a, float b, float c, float d);
    4.59 +	Plane(const Vector3 &pos, const Vector3 &norm);
    4.60 +	Plane(const Vector3 &p1, const Vector3 &p2, const Vector3 &p3);
    4.61 +
    4.62 +	bool intersect(const Ray &ray, HitPoint *hit = 0) const;
    4.63 +	bool collide(const GeomShape *geom, HitPoint *hit = 0) const;
    4.64 +
    4.65 +	float distance(const Vector3 &pt) const;
    4.66 +	float distance_sq(const Vector3 &pt) const;
    4.67 +};
    4.68 +
    4.69 +#endif	// GEOM_H_
     5.1 --- a/src/gobj.cc	Sat Feb 14 07:27:12 2015 +0200
     5.2 +++ b/src/gobj.cc	Sat Feb 14 10:08:00 2015 +0200
     5.3 @@ -78,9 +78,7 @@
     5.4  		sorted = true;
     5.5  	}
     5.6  
     5.7 -	printf("obj(%p) update\n", (void*)this);
     5.8  	for(size_t i=0; i<comp.size(); i++) {
     5.9 -		printf("  updating component: %s\n", comp[i]->get_name());
    5.10  		comp[i]->update(dt);
    5.11  	}
    5.12  }
     6.1 --- a/src/sim.cc	Sat Feb 14 07:27:12 2015 +0200
     6.2 +++ b/src/sim.cc	Sat Feb 14 10:08:00 2015 +0200
     6.3 @@ -9,7 +9,7 @@
     6.4  
     6.5  void SimWorld::add_object(GObject *obj)
     6.6  {
     6.7 -	CoRigid *co = COCAST(CoRigid, obj->get_component("phys"));
     6.8 +	CoRigid *co = COCAST(CoRigid, obj->get_component("rigid"));
     6.9  	if(co) {
    6.10  		add_rigid_body(co);
    6.11  	} else {
    6.12 @@ -27,7 +27,7 @@
    6.13  
    6.14  void SimWorld::remove_object(GObject *obj)
    6.15  {
    6.16 -	CoRigid *co = COCAST(CoRigid, obj->get_component("phys"));
    6.17 +	CoRigid *co = COCAST(CoRigid, obj->get_component("rigid"));
    6.18  	if(co) {
    6.19  		remove_rigid_body(co);
    6.20  	} else {
     7.1 --- a/src/test.cc	Sat Feb 14 07:27:12 2015 +0200
     7.2 +++ b/src/test.cc	Sat Feb 14 10:08:00 2015 +0200
     7.3 @@ -37,7 +37,7 @@
     7.4  	glutCreateWindow("component system test");
     7.5  
     7.6  	glutDisplayFunc(display);
     7.7 -	//glutIdleFunc(idle);
     7.8 +	glutIdleFunc(idle);
     7.9  	glutReshapeFunc(reshape);
    7.10  	glutKeyboardFunc(keyb);
    7.11  	glutMouseFunc(mouse);
    7.12 @@ -67,8 +67,9 @@
    7.13  	obj->add_component(new CoXForm);
    7.14  	obj->add_component(new CoPRS);
    7.15  	obj->add_component(new CoRigid);
    7.16 -	COCAST(CoPRS, obj->get_component("prs"))->pos = Vector3(5, 1, 0);
    7.17 +	COCAST(CoPRS, obj->get_component("prs"))->pos = Vector3(5, 2, 0);
    7.18  	objects.push_back(obj);
    7.19 +	simworld.add_object(obj);
    7.20  
    7.21  	obj = new GObject;
    7.22  	obj->add_component(new CoSphereVis);