Some collision work

include/anki/Collision.h

@@ -16,6 +16,7 @@
 #include "anki/collision/Frustum.h"
 #include "anki/collision/Aabb.h"
 #include "anki/collision/CompoundShape.h"
+#include "anki/collision/ConvexHullShape.h"
 
 #include "anki/collision/GjkEpa.h"
 #include "anki/collision/Functions.h"

include/anki/collision/ConvexHullShape.h

@@ -37,7 +37,10 @@ public:
 
 	ConvexHullShape(const ConvexHullShape& b) = delete;
 
-	~ConvexHullShape();
+	~ConvexHullShape()
+	{
+		destroy();
+	}
 
 	ConvexHullShape& operator=(const ConvexHullShape& b) = delete;
 
@@ -47,6 +50,24 @@ public:
 		return *this;
 	}
 
+	/// Get points in local space.
+	const Vec4* getPoints() const
+	{
+		ANKI_ASSERT(m_points);
+		return m_points;
+	}
+
+	U32 getPointsCount() const
+	{
+		ANKI_ASSERT(m_pointsCount > 0);
+		return m_pointsCount;
+	}
+
+	const Transform& getTransform() const
+	{
+		return m_trf;
+	}
+
 	/// Calculate from a set of points. You have to call initStorage before
 	/// calling this method.
 	void setFromPointCloud(
@@ -98,6 +119,8 @@ private:
 	Bool8 m_ownsTheStorage = false;
 
 	void move(ConvexHullShape& b);
+
+	void destroy();
 };
 /// @}
 

include/anki/renderer/DebugDrawer.h

@@ -131,6 +131,8 @@ public:
 	void visit(const CompoundShape&)
 	{}
 
+	void visit(const ConvexHullShape&);
+
 private:
 	DebugDrawer* m_dbg; ///< The debug drawer
 };

src/collision/ConvexHullShape.cpp

@@ -5,11 +5,13 @@
 
 #include "anki/collision/ConvexHullShape.h"
 #include "anki/collision/Plane.h"
+#include "anki/collision/Aabb.h"
+#include "anki/util/Functions.h"
 
 namespace anki {
 
 //==============================================================================
-ConvexHullShape::~ConvexHullShape()
+void ConvexHullShape::destroy()
 {
 	if(m_ownsTheStorage)
 	{
@@ -29,7 +31,7 @@ ConvexHullShape::~ConvexHullShape()
 //==============================================================================
 void ConvexHullShape::move(ConvexHullShape& b)
 {
-	this->~ConvexHullShape();
+	destroy();
 	Base::operator=(b);
 
 	m_trf = b.m_trf;
@@ -54,7 +56,7 @@ void ConvexHullShape::move(ConvexHullShape& b)
 //==============================================================================
 Error ConvexHullShape::initStorage(CollisionAllocator<U8>& alloc, U pointCount)
 {
-	this->~ConvexHullShape();
+	destroy();
 
 	m_points = 
 		reinterpret_cast<Vec4*>(alloc.allocate(pointCount * sizeof(Vec4)));
@@ -78,7 +80,7 @@ void ConvexHullShape::initStorage(void* buffer, U pointCount)
 	ANKI_ASSERT(buffer);
 	ANKI_ASSERT(pointCount > 0);
 	
-	this->~ConvexHullShape();
+	destroy();
 	m_points = static_cast<Vec4*>(buffer);
 	m_pointsCount = pointCount;
 	ANKI_ASSERT(m_ownsTheStorage == false);
@@ -119,6 +121,46 @@ void ConvexHullShape::transform(const Transform& trf)
 //==============================================================================
 void ConvexHullShape::computeAabb(Aabb& aabb) const
 {
+	ANKI_ASSERT(m_points);
+	
+	Vec3 mina(MAX_F32);
+	Vec3 maxa(MIN_F32);
+	const Vec4* points = m_points;
+	const Vec4* end = m_points + m_pointsCount;
+	for(; points != end; ++points)
+	{
+		Vec4 o = m_trf.transform(*points);
+		for(U i = 0; i < 3; ++i)
+		{
+			mina[i] = min(mina[i], o[i]);
+			maxa[i] = max(maxa[i], o[i]);
+		}
+	}
+
+	aabb = Aabb(mina.xyz0(), maxa.xyz0());
+}
+
+//==============================================================================
+Vec4 ConvexHullShape::computeSupport(const Vec4& dir) const
+{
+	F32 m = MIN_F32;
+	U index = 0;
+
+	const Vec4* points = m_points;
+	const Vec4* end = m_points + m_pointsCount;
+	for(; points != end; ++points)
+	{
+		// XXX Optimize
+		Vec4 o = m_trf.transform(*points);
+		F32 dot = o.dot(dir);
+		if(dot > m)
+		{
+			m = dot;
+			index = 0;
+		}
+	}
+
+	return m_points[index];
 }
 
 } // end namespace anki

src/renderer/Dbg.cpp

@@ -12,6 +12,7 @@
 #include "anki/util/Logger.h"
 #include "anki/util/Enum.h"
 #include "anki/misc/ConfigSet.h"
+#include "anki/collision/ConvexHullShape.h"
 
 namespace anki {
 
@@ -136,316 +137,45 @@ Error Dbg::run(GlCommandBufferHandle& cmdb)
 		phyd.drawWorld(scene._getPhysicsWorld());
 	}
 
-	// XXX
-#if 0
-	if(0)
+#if 1
 	{
-		Vec3 tri[3] = {
-			Vec3{10.0 , 7.0, -2.0}, 
-			Vec3{-10.0 , 7.0, -2.0}, 
-			Vec3{-10.0 , 0.0, -2.0}};
+		ConvexHullShape hull;
+		Vec4 storage[] = {
+			Vec4(1.0, 1.0, 1.0, 0.0),
+			Vec4(1.0, 1.0, -1.0, 0.0),
+			Vec4(-1.0, 1.0, -1.0, 0.0),
+			Vec4(-1.0, -1.0, 1.0, 0.0),
+		};
 
-		Mat4 mvp = scene.getActiveCamera().getViewProjectionMatrix();
+		SceneNode& sn = scene.findSceneNode("horse");
+		MoveComponent& move = sn.getComponent<MoveComponent>();
 
-		Array<Vec4, 3> projtri = {{
-			mvp * Vec4(tri[0], 1.0), 
-			mvp * Vec4(tri[1], 1.0), 
-			mvp * Vec4(tri[2], 1.0)}};
+		hull.initStorage(storage, 4);
+		hull.transform(move.getWorldTransform());
 
-		Array<Vec4, 3> cliptri;
+		Sphere s(Vec4(0.0), 1.0);
 
-		for(int i = 0; i < 3; i++)
-		{
-			Vec4 proj = projtri[i];
-			cliptri[i] = proj / fabs(proj.w());
-		}
-
-		Vec3 a = cliptri[1].xyz() - cliptri[0].xyz();
-		Vec3 b = cliptri[2].xyz() - cliptri[1].xyz();
-		//Vec2 c = a.xy() * b.yx();
-		//Bool frontfacing = (c.x() - c.y()) > 0.0;
-		Vec3 cross = a.cross(b);
-		Bool frontfacing = cross.z() >= 0.0;
-
-		if(frontfacing)
-		{
-			drawer->setColor(Vec4(1.0));
-			drawer->setViewProjectionMatrix(
-				scene.getActiveCamera().getViewProjectionMatrix());
-			drawer->setModelMatrix(Mat4::getIdentity());
-
-			drawer->pushBackVertex(tri[0]);
-			drawer->pushBackVertex(tri[1]);
-			drawer->pushBackVertex(tri[1]);
-			drawer->pushBackVertex(tri[2]);
-			drawer->pushBackVertex(tri[2]);
-			drawer->pushBackVertex(tri[0]);
-		}
-
-		{
-			for(Vec4& v : cliptri)
-			{
-				std::cout << v.toString() << "\n";
-			}
-
-			std::cout << std::endl;
-
-			std::cout << a.toString() << ", " << b.toString() 
-				//<< ", " << c.toString() << ", " 
-				//<< (c.x() - c.y()) 
-				<< ", ff:" << (U)frontfacing
-				<< std::endl;
-
-			std::cout << std::endl;
-		}
+		Gjk gjk;
+		Bool collide = gjk.intersect(s, hull);
 
-		{
-			drawer->setColor(Vec4(0.0, 1.0, 0.0, 1.0));
-			drawer->setViewProjectionMatrix(Mat4::getIdentity());
-			drawer->setModelMatrix(Mat4::getIdentity());
-
-			F32 z = 0.0;
-
-			drawer->pushBackVertex(Vec3(cliptri[0].xy(), z));
-			drawer->pushBackVertex(Vec3(cliptri[1].xy(), z));
-			drawer->pushBackVertex(Vec3(cliptri[1].xy(), z));
-			drawer->pushBackVertex(Vec3(cliptri[2].xy(), z));
-		}
-	}
-	// XXX
-#endif
-
-#if 0
-	{
-		Vec4 pos0 = scene.findSceneNode("shape0").
-			getComponent<MoveComponent>().getWorldTransform().getOrigin();
-
-		Vec4 pos1 = scene.findSceneNode("shape1").
-			getComponent<MoveComponent>().getWorldTransform().getOrigin();
-		Mat3x4 rot1 = scene.findSceneNode("shape1").
-			getComponent<MoveComponent>().getWorldTransform().getRotation();
-
-		Aabb s0(pos0 - Vec4(1.0, 1.0, 2.0, 0.0), pos0 + Vec4(1.0, 1.0, 2.0, 0.0));
-		Obb s1(pos1, rot1, Vec4(1.0, 0.5, 2.5, 0.0));
-
-		CollisionDebugDrawer dr(m_drawer);
-
-		GjkEpa gjk(100, 100, 100);
-		ContactPoint cp;
-		StackAllocator<U8> alloc(
-			StackMemoryPool(allocAligned, nullptr, 1024 * 1024));
-
-		Bool intersect = gjk.intersect(s0, s1, cp, alloc);
-
-		if(intersect)
+		if(collide)
 		{
 			m_drawer->setColor(Vec4(1.0, 0.0, 0.0, 1.0));
 		}
 		else
 		{
-			m_drawer->setColor(Vec4(1.0, 1.0, 1.0, 1.0));
-		}
-
-		s0.accept(dr);
-		s1.accept(dr);
-
-
-		if(intersect) {
-
-
-		if(1)
-		{
-		m_drawer->setModelMatrix(Mat4::getIdentity());
-		m_drawer->setColor(Vec4(0.0, 1.0, 0.0, 1.0));
-		m_drawer->begin(GL_LINES);
-		m_drawer->pushBackVertex(Vec3(0.0));
-		m_drawer->pushBackVertex(cp.m_normal.xyz() * cp.m_depth);
-		//m_drawer->pushBackVertex(cp.m_normal.xyz() * 2.0);
-		//m_drawer->pushBackVertex(gjk.m_faces[gjk.m_faceCount - 3].m_normal.xyz());
-		m_drawer->end();
-		}
-
-#if 0
-		if(0)
-		{
-			m_drawer->setModelMatrix(Mat4::getIdentity());
 			m_drawer->setColor(Vec4(1.0));
-
-			m_drawer->begin(GL_LINES);
-			m_drawer->pushBackVertex(Vec3(-10, 0, 0));
-			m_drawer->pushBackVertex(Vec3(10, 0, 0));
-			m_drawer->pushBackVertex(Vec3(0, 0, -10));
-			m_drawer->pushBackVertex(Vec3(0, 0, 10));
-			m_drawer->end();
-		}
-
-		if(0)
-		{
-			m_drawer->setColor(Vec4(.0, 1.0, 1.0, 1.0));
-			m_drawer->pushBackVertex(Vec3(0, 0, 0));
-			m_drawer->pushBackVertex(gjk.m_poly->m_simplex[0].m_v.xyz());
-			std::cout << gjk.m_poly->m_simplex[0].m_v.xyz().toString() << std::endl;
-		}
-
-		if(0)
-		{
-			m_drawer->setColor(Vec4(0.0, 1.0, 1.0, 1.0));
-
-			Vec4 n = Vec4(0.294484, 0.239468, 0.925074, 0.000000);
-			Vec4 p0 = Vec4(0.777393, 0.538571, 0.068147, 0);
-
-			Vec4 p = Vec4(-0.296335, -0.886740, 1.415587, 0.0);
-
-			m_drawer->pushBackVertex(Vec3(0, 0, 0));
-			m_drawer->pushBackVertex(p.xyz());
-
-			p = p - n.dot(p - p0) * n;
-			m_drawer->pushBackVertex(Vec3(0, 0, 0));
-			m_drawer->pushBackVertex(p.xyz());
 		}
 
-		if(0)
-		{
-			Mat4 m(Vec4(0.0, 0.0, 0.0, 1.0), Mat3::getIdentity(), 1.0);
-			m_drawer->setModelMatrix(m);
-			m_drawer->setColor(Vec4(1.0, 0.0, 1.0, 1.0));
-			m_drawer->begin(GL_LINES);
-			Array<U, 12> idx = {{0, 1, 2, 1, 2, 3, 2, 3, 0, 3, 0, 1}};
-			for(U i = 0; i < idx.size(); i += 3)
-			{
-				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 0]].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 1]].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 1]].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 2]].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 2]].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[idx[i + 0]].m_v.xyz());
-			}
-			m_drawer->end();
-		}
-
-		if(0)
-		{
-			Mat4 m(Vec4(0.0, 0.0, 0.0, 1.0), Mat3::getIdentity(), 1.02);
-			m_drawer->setModelMatrix(m);
-			m_drawer->setColor(Vec4(1.0, 1.0, 0.0, 1.0));
-			m_drawer->begin(GL_LINES);
-			for(U i = 0; i < gjk.m_poly->m_simplex.size() - 1; i++)
-			{
-				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[i].m_v.xyz());
-				m_drawer->pushBackVertex(gjk.m_poly->m_simplex[i + 1].m_v.xyz());
-			}
-			m_drawer->end();
-		}
-
-		if(1)
-		{
-			Mat4 m(Vec4(0.0, 0.0, 0.0, 1.0), Mat3::getIdentity(), 1.0);
-			m_drawer->setModelMatrix(m);
-			m_drawer->begin(GL_TRIANGLES);
-
-			static U64 count = 0;
-
-			++count;
-			U faceCount = gjk.m_poly->m_faces.size();
-			U offset = (count / 80) % faceCount;
-			//for(U i = 0; i < 1; i++)
-			for(U i = 0; i < faceCount; i++)
-			{
-				if(gjk.m_poly->m_faces[i].dead())
-				{
-					continue;
-				}
-
-				//auto idx = gjk.m_faces[offset].m_idx;
-				auto idx = gjk.m_poly->m_faces[i].idx();
-
-				/*if(i % 2)
-				{
-					m = Mat4(Vec4(0.0, 0.0, 0.0, 1.0), Mat3::getIdentity(), 1.01);
-				}
-				else
-				{
-					m = Mat4(Vec4(0.0, 0.0, 0.0, 1.0), Mat3::getIdentity(), 1.0);
-				}
-				m_drawer->setModelMatrix(m);*/
-
-				if(i == faceCount - 1)
-				{
-					m_drawer->setColor(Vec4(1.0, 1.0, 1.0, 1.0));
-				}
-				else
-				{
-					m_drawer->setColor(Vec4(1.0, 0.0, 1.0, 1.0));
-				}
-
-				#define WHAT(i_) gjk.m_poly->m_simplex[idx[i_]].m_v.xyz()
-
-				m_drawer->setColor(Vec4(1.0, 0.0, 0.0, 1.0));
-				m_drawer->pushBackVertex(WHAT(0));
-				m_drawer->setColor(Vec4(0.0, 1.0, 0.0, 1.0));
-				m_drawer->pushBackVertex(WHAT(1));
-				m_drawer->setColor(Vec4(0.0, 0.0, 1.0, 1.0));
-				m_drawer->pushBackVertex(WHAT(2));
-
-
-				m_drawer->setColor(Vec4(1.0, 0.0, 0.0, 1.0) / 3);
-				m_drawer->pushBackVertex(WHAT(2));
-				m_drawer->setColor(Vec4(0.0, 1.0, 0.0, 1.0) / 3);
-				m_drawer->pushBackVertex(WHAT(1));
-				m_drawer->setColor(Vec4(0.0, 0.0, 1.0, 1.0) / 3);
-				m_drawer->pushBackVertex(WHAT(0));
-
-				#undef WHAT
-			}
-
-			m_drawer->end();
-		}
-
-		if(0)
-		{
-			Mat4 m(Vec4(0.0, 0.0, 0.0, 1.0), Mat3::getIdentity(), 1.0);
-			m_drawer->setModelMatrix(m);
-			m_drawer->begin(GL_LINES);
-			m_drawer->setColor(Vec4(1.));
-
-			m_drawer->pushBackVertex(Vec3(0.));
-			m_drawer->pushBackVertex(gjk.m_poly->m_simplex.back().m_v.xyz() * 1.1);
-			
-		}
-
-		if(0)
-		{
-
-			Mat4 m(Vec4(0.0, 0.0, 0.0, 1.0), Mat3::getIdentity(), 1.0);
-			m_drawer->setModelMatrix(m);
-			m_drawer->begin(GL_LINES);
-			m_drawer->setColor(Vec4(1.));
-
-			Vec4 support = gjk.m_poly->m_simplex.back().m_v;
-			Vec4 normal = gjk.m_poly->m_faces[3].normal(*gjk.m_poly);
-
-			std::cout << normal.dot(support) << std::endl;
-
-			m_drawer->pushBackVertex(Vec3(0.));
-			m_drawer->pushBackVertex(
-				 normal.xyz() * 2.1);
-			
-		}
-
-
-		/*m_drawer->setColor(Vec4(0.0, 1.0, 0.0, 1.0));
 		m_drawer->setModelMatrix(Mat4::getIdentity());
-		m_drawer->begin();
-		m_drawer->pushBackVertex(gjk.m_a.xyz());
-		m_drawer->pushBackVertex(gjk.m_b.xyz());
-		m_drawer->pushBackVertex(gjk.m_b.xyz());
-		m_drawer->pushBackVertex(gjk.m_c.xyz());
-		m_drawer->end();*/
-
-		alloc.getMemoryPool().reset();
-#endif
+		CollisionDebugDrawer cd(m_drawer);
+
+		cd.visit(hull);
+		cd.visit(s);
 
-		} // intersect
+		Aabb aabb;
+		hull.computeAabb(aabb);
+		cd.visit(aabb);
 	}
 #endif
 

src/renderer/DebugDrawer.cpp

@@ -475,6 +475,21 @@ void CollisionDebugDrawer::visit(const Frustum& f)
 	}
 }
 
+//==============================================================================
+void CollisionDebugDrawer::visit(const ConvexHullShape& hull)
+{
+	m_dbg->setModelMatrix(Mat4(hull.getTransform()));
+	m_dbg->begin(GL_LINES);
+	const Vec4* points = hull.getPoints();
+	const Vec4* end = points + hull.getPointsCount();
+	for(; points != end; ++points)
+	{
+		m_dbg->pushBackVertex(Vec3(0.0));
+		m_dbg->pushBackVertex(points->xyz());
+	}
+	m_dbg->end();
+}
+
 //==============================================================================
 // PhysicsDebugDrawer                                                          =
 //==============================================================================