From: Mikko Rasa <tdb@tdb.fi>
Date: Sun, 19 May 2013 18:32:09 +0000 (+0300)
Subject: Add more collision check functions for shapes
X-Git-Url: http://git.tdb.fi/?a=commitdiff_plain;h=9260eee3126731c1e6b54ce3407be757d0d71716;p=libs%2Fmath.git

Add more collision check functions for shapes
---

diff --git a/source/geometry/hyperbox.h b/source/geometry/hyperbox.h
index 2ade28a..6248d6b 100644
--- a/source/geometry/hyperbox.h
+++ b/source/geometry/hyperbox.h
@@ -1,9 +1,12 @@
 #ifndef MSP_GEOMETRY_HYPERBOX_H_
 #define MSP_GEOMETRY_HYPERBOX_H_
 
+#include <algorithm>
+#include <cmath>
 #include <msp/linal/vector.h>
 #include "ray.h"
 #include "shape.h"
+#include "surfacepoint.h"
 
 namespace Msp {
 namespace Geometry {
@@ -24,7 +27,10 @@ public:
 	T get_dimension(unsigned) const;
 
 	virtual HyperBox<T, D> get_axis_aligned_bounding_box() const { return *this; }
+	virtual bool contains(const LinAl::Vector<T, D> &) const;
 	virtual bool check_intersection(const Ray<T, D> &) const;
+	virtual unsigned get_max_ray_intersections() const { return 2; }
+	virtual unsigned get_intersections(const Ray<T, D> &, SurfacePoint<T, D> *, unsigned) const;
 };
 
 template<typename T, unsigned D>
@@ -51,26 +57,67 @@ inline T HyperBox<T, D>::get_dimension(unsigned i) const
 	return dimensions[i];
 }
 
+template<typename T, unsigned D>
+inline bool HyperBox<T, D>::contains(const LinAl::Vector<T, D> &point) const
+{
+	for(unsigned i=0; i<D; ++i)
+		if(abs(point[i])>dimensions[i]/2)
+			return false;
+	return true;
+}
+
 template<typename T, unsigned D>
 inline bool HyperBox<T, D>::check_intersection(const Ray<T, D> &ray) const
 {
+	return get_intersections(ray, 0, 1);
+}
+
+template<typename T, unsigned D>
+inline unsigned HyperBox<T, D>::get_intersections(const Ray<T, D> &ray, SurfacePoint<T, D> *points, unsigned size) const
+{
+	using std::abs;
+
 	LinAl::Vector<T, D> half_dim = dimensions/T(2);
+	unsigned n = 0;
+	T first_depth = T();
 	for(unsigned i=0; i<D; ++i)
+	{
+		if(!ray.get_direction()[i])
+			continue;
+
 		for(int j=-1; j<=1; j+=2)
 		{
 			T x = (T(j)*half_dim[i]-ray.get_start()[i])/ray.get_direction()[i];
-			if(x>0)
+			if(x<0)
+				continue;
+
+			LinAl::Vector<T, D> p = ray.get_start()+ray.get_direction()*x;
+
+			bool inside = true;
+			for(unsigned k=0; (inside && k<D); ++k)
+				inside = (k==i || abs(p[k])<=half_dim[k]);
+
+			if(inside && n<size)
 			{
-				LinAl::Vector<T, D> p = ray.get_start()+ray.get_direction()*x;
-				bool inside = true;
-				for(unsigned k=0; (inside && k<D); ++k)
-					inside = (k==i || (p[k]>=-half_dim[k] && p[k]<half_dim[k]));
-				if(inside)
-					return true;
+				if(points)
+				{
+					points[n].position = p;
+					points[n].normal = LinAl::Vector<T, D>();
+					points[n].normal[i] = j;
+					if(n==0)
+						first_depth = x;
+					else if(n==1 && x<first_depth)
+						std::swap(points[0], points[1]);
+				}
+
+				++n;
+				if(n==size || n==2)
+					return n;
 			}
 		}
+	}
 
-	return false;
+	return n;
 }
 
 } // namespace Geometry
diff --git a/source/geometry/hypersphere.h b/source/geometry/hypersphere.h
index 8b7d0a4..f95ebe4 100644
--- a/source/geometry/hypersphere.h
+++ b/source/geometry/hypersphere.h
@@ -5,6 +5,7 @@
 #include "hyperbox.h"
 #include "ray.h"
 #include "shape.h"
+#include "surfacepoint.h"
 
 namespace Msp {
 namespace Geometry {
@@ -24,7 +25,10 @@ public:
 	T get_radius() const { return radius; }
 
 	virtual HyperBox<T, D> get_axis_aligned_bounding_box() const;
+	virtual bool contains(const LinAl::Vector<T, D> &) const;
 	virtual bool check_intersection(const Ray<T, D> &) const;
+	virtual unsigned get_max_ray_intersections() const { return 2; }
+	virtual unsigned get_intersections(const Ray<T, D> &, SurfacePoint<T, D> *, unsigned) const;
 };
 
 template<typename T, unsigned D>
@@ -52,17 +56,51 @@ inline HyperBox<T, D> HyperSphere<T, D>::get_axis_aligned_bounding_box() const
 	return HyperBox<T, D>(dimensions);
 }
 
+template<typename T, unsigned D>
+inline bool HyperSphere<T, D>::contains(const LinAl::Vector<T, D> &point) const
+{
+	return inner_product(point, point)<=radius*radius;
+}
+
 template<typename T, unsigned D>
 inline bool HyperSphere<T, D>::check_intersection(const Ray<T, D> &ray) const
 {
 	T x = inner_product(ray.get_direction(), ray.get_start());
 	if(x>0)
-		return inner_product(ray.get_start(), ray.get_start())<=radius*radius;
+		return contains(ray.get_start());
 	else
+		return contains(ray.get_start()-ray.get_direction()*x);
+}
+
+template<typename T, unsigned D>
+inline unsigned HyperSphere<T, D>::get_intersections(const Ray<T, D> &ray, SurfacePoint<T, D> *points, unsigned size) const
+{
+	T mid = -inner_product(ray.get_direction(), ray.get_start());
+	LinAl::Vector<T, D> nearest = ray.get_start()+ray.get_direction()*mid;
+	T offset_sq = radius*radius-inner_product(nearest, nearest);
+	if(offset_sq<0)
+		return 0;
+	T offset = sqrt(offset_sq);
+
+	unsigned n = 0;
+	for(int i=-1; i<=1; i+=2)
 	{
-		LinAl::Vector<T, D> nearest = ray.get_start()-ray.get_direction()*x;
-		return inner_product(nearest, nearest)<=radius*radius;
+		T x = mid+offset*i;
+		if(x>0 && n<size)
+		{
+			if(points)
+			{
+				points[n].position = ray.get_start()+ray.get_direction()*x;
+				points[n].normal = normalize(points[n].position);
+			}
+
+			++n;
+			if(n==size)
+				return n;
+		}
 	}
+
+	return n;
 }
 
 } // namespace Geometry
diff --git a/source/geometry/shape.h b/source/geometry/shape.h
index 528a77c..1f81764 100644
--- a/source/geometry/shape.h
+++ b/source/geometry/shape.h
@@ -1,6 +1,9 @@
 #ifndef MSP_GEOMETRY_SHAPE_H_
 #define MSP_GEOMETRY_SHAPE_H_
 
+#include <vector>
+#include <msp/linal/vector.h>
+
 namespace Msp {
 namespace Geometry {
 
@@ -10,6 +13,9 @@ class HyperBox;
 template<typename T, unsigned D>
 class Ray;
 
+template<typename T, unsigned D>
+class SurfacePoint;
+
 template<typename T, unsigned D>
 class Shape
 {
@@ -21,9 +27,23 @@ public:
 	virtual Shape *clone() const = 0;
 
 	virtual HyperBox<T, D> get_axis_aligned_bounding_box() const = 0;
+	virtual bool contains(const LinAl::Vector<T, D> &) const = 0;
 	virtual bool check_intersection(const Ray<T, D> &) const = 0;
+	virtual unsigned get_max_ray_intersections() const = 0;
+	virtual unsigned get_intersections(const Ray<T, D> &, SurfacePoint<T, D> *, unsigned) const = 0;
+	std::vector<SurfacePoint<T, D> > get_intersections(const Ray<T, D> &) const;
 };
 
+template<typename T, unsigned D>
+std::vector<SurfacePoint<T, D> > Shape<T, D>::get_intersections(const Ray<T, D> &ray) const
+{
+	unsigned max_isect = get_max_ray_intersections();
+	std::vector<SurfacePoint<T, D> > points(max_isect);
+	unsigned count = get_intersections(ray, &points[0], max_isect);
+	points.resize(count);
+	return points;
+}
+
 } // namespace Geometry
 } // namespace Msp
 
diff --git a/source/geometry/surfacepoint.h b/source/geometry/surfacepoint.h
new file mode 100644
index 0000000..3513ac8
--- /dev/null
+++ b/source/geometry/surfacepoint.h
@@ -0,0 +1,19 @@
+#ifndef MSP_GEOMETRY_SURFACEPOINT_H_
+#define MSP_GEOMETRY_SURFACEPOINT_H_
+
+#include <msp/linal/vector.h>
+
+namespace Msp {
+namespace Geometry {
+
+template<typename T, unsigned N>
+struct SurfacePoint
+{
+	LinAl::Vector<T, N> position;
+	LinAl::Vector<T, N> normal;
+};
+
+} // namespace Geometry
+} // namespace Msp
+
+#endif
diff --git a/source/geometry/transformedshape.h b/source/geometry/transformedshape.h
index 2168126..ea7281f 100644
--- a/source/geometry/transformedshape.h
+++ b/source/geometry/transformedshape.h
@@ -28,7 +28,10 @@ public:
 	const AffineTransformation<T, D> &get_transformation() const { return transformation; }
 
 	virtual HyperBox<T, D> get_axis_aligned_bounding_box() const;
+	virtual bool contains(const LinAl::Vector<T, D> &) const;
 	virtual bool check_intersection(const Ray<T, D> &) const;
+	virtual unsigned get_max_ray_intersections() const { return shape->get_max_ray_intersections(); }
+	virtual unsigned get_intersections(const Ray<T, D> &, SurfacePoint<T, D> *, unsigned) const;
 };
 
 template<typename T, unsigned D>
@@ -73,6 +76,12 @@ inline HyperBox<T, D> TransformedShape<T, D>::get_axis_aligned_bounding_box() co
 	return shape->get_axis_aligned_bounding_box();
 }
 
+template<typename T, unsigned D>
+inline bool TransformedShape<T, D>::contains(const LinAl::Vector<T, D> &point) const
+{
+	return shape->contains(inverse_trans.transform(point));
+}
+
 template<typename T, unsigned D>
 inline bool TransformedShape<T, D>::check_intersection(const Ray<T, D> &ray) const
 {
@@ -81,6 +90,25 @@ inline bool TransformedShape<T, D>::check_intersection(const Ray<T, D> &ray) con
 	return shape->check_intersection(local_ray);
 }
 
+template<typename T, unsigned D>
+inline unsigned TransformedShape<T, D>::get_intersections(const Ray<T, D> &ray, SurfacePoint<T, D> *points, unsigned size) const
+{
+	Ray<T, D> local_ray(inverse_trans.transform(ray.get_start()),
+		inverse_trans.transform_linear(ray.get_direction()));
+	unsigned count = shape->get_intersections(local_ray, points, size);
+	if(points)
+	{
+		for(unsigned i=0; i<count; ++i)
+		{
+			points[i].position = transformation.transform(points[i].position);
+			/* XXX This is not correct for nonuniform scaling.  Inverse of the
+			transpose of the upper DxD part of the matrix should be used. */
+			points[i].normal = transformation.transform(points[i].normal);
+		}
+	}
+	return count;
+}
+
 } // namespace Geometry
 } // namespace Msp