1 #pragma once
2 //#include <ostream>
3 
4 #include "mathvector.h"
5 #include "mathplane.h"
6 
7 template <typename T>
8 class AABB
9 {
10 private:
11 	MATHVECTOR<T,3> pos; ///< minimum corner (center-size*0.5)
12 	MATHVECTOR<T,3> center; ///< exact center of AABB
13 	MATHVECTOR<T,3> size; ///< size of AABB
14 
15 public:
AABB(const AABB<T> & other)16 	AABB(const AABB <T> & other) : pos(other.pos),center(other.center),size(other.size) {}
AABB()17 	AABB() {}
18 
19 	const AABB <T> & operator = (const AABB <T> & other)
20 	{
21 		pos = other.pos;
22 		center = other.center;
23 		size = other.size;
24 
25 		return *this;
26 	}
27 
GetPos()28 	const MATHVECTOR<T,3> & GetPos() const {return pos;}
GetSize()29 	const MATHVECTOR<T,3> & GetSize() const {return size;}
GetCenter()30 	const MATHVECTOR<T,3> & GetCenter() const {return center;}
31 
DebugPrint(std::ostream & o)32 	void DebugPrint(std::ostream & o) const
33 	{
34 		MATHVECTOR<T,3> min(pos);
35 		MATHVECTOR<T,3> max(pos+size);
36 		o << min[0] << "," << min[1] << "," << min[2] << " to " << max[0] << "," << max[1] << "," << max[2] << std::endl;
37 	}
38 
DebugPrint2(std::ostream & o)39 	void DebugPrint2(std::ostream & o) const
40 	{
41 		o << "center: " << center[0] << "," << center[1] << "," << center[2] << " size: " << size[0] << "," << size[1] << "," << size[2] << std::endl;
42 	}
43 
SetFromCorners(const MATHVECTOR<T,3> & c1,const MATHVECTOR<T,3> & c2)44 	void SetFromCorners(const MATHVECTOR<T,3> & c1, const MATHVECTOR<T,3> & c2)
45 	{
46 		MATHVECTOR<T,3> c1mod(c1);
47 		MATHVECTOR<T,3> c2mod(c2);
48 
49 		//ensure c1 is smaller than c2
50 		if (c1[0] > c2[0])
51 		{
52 			c1mod[0] = c2[0];
53 			c2mod[0] = c1[0];
54 		}
55 		if (c1[1] > c2[1])
56 		{
57 			c1mod[1] = c2[1];
58 			c2mod[1] = c1[1];
59 		}
60 		if (c1[2] > c2[2])
61 		{
62 			c1mod[2] = c2[2];
63 			c2mod[2] = c1[2];
64 		}
65 
66 		/*float fluff = 0.0001;
67 		VERTEX fluffer;
68 		fluffer.Set(fluff, fluff, fluff);
69 
70 		//add in some fudge factor
71 		c1mod = c1mod + fluffer.ScaleR(-1.0);
72 		c2mod = c2mod + fluffer;*/
73 
74 		pos = c1mod;
75 		size = c2mod - c1mod;
76 		center = pos + size * 0.5;
77 	}
78 
CombineWith(const AABB<T> & other)79 	void CombineWith(const AABB <T> & other)
80 	{
81 		const MATHVECTOR<T,3> & otherpos(other.GetPos());
82 		MATHVECTOR<T,3> min(0), max(0);
83 		min = pos;
84 		max = pos + size;
85 		if (otherpos[0] < min[0])
86 			min[0] = otherpos[0];
87 		if (otherpos[1] < min[1])
88 			min[1] = otherpos[1];
89 		if (otherpos[2] < min[2])
90 			min[2] = otherpos[2];
91 
92 		const MATHVECTOR<T,3> & othermax(otherpos + other.GetSize());
93 		if (othermax[0] > max[0])
94 			max[0] = othermax[0];
95 		if (othermax[1] > max[1])
96 			max[1] = othermax[1];
97 		if (othermax[2] > max[2])
98 			max[2] = othermax[2];
99 
100 		SetFromCorners(min, max);
101 	}
102 
103 	class RAY
104 	{
105 		public:
RAY(const MATHVECTOR<T,3> & neworig,const MATHVECTOR<T,3> & newdir,T newseglen)106 			RAY(const MATHVECTOR<T,3> & neworig, const MATHVECTOR<T,3> & newdir, T newseglen) : orig(neworig), dir(newdir), seglen(newseglen) {}
107 
108 			MATHVECTOR<T,3> orig;
109 			MATHVECTOR<T,3> dir;
110 			T seglen;
111 	};
112 
Intersect(const RAY & ray)113 	bool Intersect(const RAY & ray) const
114 	{
115 		//if (seglen>3e30f) return IntersectRay(orig, dir); // infinite ray
116 		MATHVECTOR<T,3> segdir(ray.dir * (0.5f * ray.seglen));
117 		MATHVECTOR<T,3> seg_center(ray.orig + segdir);
118 		MATHVECTOR<T,3> diff(seg_center - center);
119 
120 		MATHVECTOR<T,3> abs_segdir(segdir);
121 		abs_segdir.absify();
122 		MATHVECTOR<T,3> abs_diff(diff);
123 		abs_diff.absify();
124 		T f = size[0] + abs_segdir[0];
125 		if (abs_diff[0] > f) return false;
126 		f = size[1] + abs_segdir[1];
127 		if (abs_diff[1] > f) return false;
128 		f = size[2] + abs_segdir[2];
129 		if (abs_diff[2] > f) return false;
130 
131 		MATHVECTOR<T,3> cross(segdir.cross(diff));
132 
133 		MATHVECTOR<T,3> abs_cross(cross);
134 		abs_cross.absify();
135 
136 		f = size[1]*abs_segdir[2] + size[2]*abs_segdir[1];
137 		if ( abs_cross[0] > f ) return false;
138 
139 		f = size[2]*abs_segdir[0] + size[0]*abs_segdir[2];
140 		if ( abs_cross[1] > f ) return false;
141 
142 		f = size[0]*abs_segdir[1] + size[1]*abs_segdir[0];
143 		if ( abs_cross[2] > f ) return false;
144 
145 		return true;
146 	}
147 
Intersect(const AABB<T> & other)148 	bool Intersect(const AABB <T> & other) const
149 	{
150 		MATHVECTOR<T,3> otherc1 = other.GetPos();
151 		MATHVECTOR<T,3> otherc2 = otherc1 + other.GetSize();
152 
153 		MATHVECTOR<T,3> c1 = pos;
154 		MATHVECTOR<T,3> c2 = pos + size;
155 
156 		//bias checks for non-collisions
157 		if (c1[0] > otherc2[0] || c2[0] < otherc1[0])
158 			return false;
159 
160 		if (c1[2] > otherc2[2] || c2[2] < otherc1[2])
161 			return false;
162 
163 		if (c1[1] > otherc2[1] || c2[1] < otherc1[1])
164 			return false;
165 
166 		return true;
167 	}
168 
169 	class FRUSTUM
170 	{
171 		public:
FRUSTUM()172 			FRUSTUM() : planes(6) {}
FRUSTUM(T cfrustum[][4])173 			FRUSTUM(T cfrustum[][4]) : planes(6)
174 			{
175 				Set(cfrustum);
176 			}
Set(T cfrustum[][4])177 			void Set(T cfrustum[][4])
178 			{
179 				for (int i = 0; i < 6; ++i)
180 					planes[i].Set(cfrustum[i]);
181 			}
182 
183 			std::vector <MATHPLANE <T> > planes;
184 
DebugPrint(std::ostream & output)185 			void DebugPrint(std::ostream & output)
186 			{
187 				for (int i = 0; i < 6; ++i)
188 					output << i << ". " << planes[i] << "\n";
189 			}
190 	};
191 
Intersect(const FRUSTUM & frustum)192 	bool Intersect(const FRUSTUM & frustum) const
193 	{
194 		float rd;
195 		const float bound = size.Magnitude()*0.5;
196 		for (int i=0; i<6; ++i)
197 		{
198 			rd = frustum.planes[i][0]*center[0]+
199 					frustum.planes[i][1]*center[1]+
200 					frustum.planes[i][2]*center[2]+
201 					frustum.planes[i][3];
202 			if (rd <= -bound)
203 				return false;
204 		}
205 		return true;
206 	}
207 };
208