1 files changed, 85 insertions, 0 deletions
diff --git a/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp b/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp
new file mode 100644
index 00000000000..40e762125ac
--- /dev/null
+++ b/extern/bullet/Bullet/CollisionShapes/ConeShape.cpp
@@ -0,0 +1,85 @@
+/*
+ * Copyright (c) 2005 Erwin Coumans http://www.erwincoumans.com
+ *
+ * Permission to use, copy, modify, distribute and sell this software
+ * and its documentation for any purpose is hereby granted without fee,
+ * provided that the above copyright notice appear in all copies.
+ * Erwin Coumans makes no representations about the suitability 
+ * of this software for any purpose.  
+ * It is provided "as is" without express or implied warranty.
+*/
+#include "ConeShape.h"
+#include "SimdPoint3.h"
+
+#ifdef WIN32
+static int coneindices[3] = {1,2,0};
+#else
+static int coneindices[3] = {2,1,0};
+#endif
+
+ConeShape::ConeShape (SimdScalar radius,SimdScalar height):
+m_radius (radius),
+m_height(height)
+{
+	SimdVector3 halfExtents;
+	m_sinAngle = (m_radius / sqrt(m_radius * m_radius + m_height * m_height));
+}
+
+
+SimdVector3 ConeShape::ConeLocalSupport(const SimdVector3& v) const
+{
+	
+	float halfHeight = m_height * 0.5f;
+
+ if (v[coneindices[1]] > v.length() * m_sinAngle)
+ {
+	SimdVector3 tmp;
+
+	tmp[coneindices[0]] = 0.f;
+	tmp[coneindices[1]] = halfHeight;
+	tmp[coneindices[2]] = 0.f;
+	return tmp;
+ }
+  else {
+    SimdScalar s = sqrtf(v[coneindices[0]] * v[coneindices[0]] + v[coneindices[2]] * v[coneindices[2]]);
+    if (s > SIMD_EPSILON) {
+      SimdScalar d = m_radius / s;
+	  SimdVector3 tmp;
+	  tmp[coneindices[0]] = v[coneindices[0]] * d;
+	  tmp[coneindices[1]] = -halfHeight;
+	  tmp[coneindices[2]] = v[coneindices[2]] * d;
+	  return tmp;
+    }
+    else  {
+		SimdVector3 tmp;
+		tmp[coneindices[0]] = 0.f;
+		tmp[coneindices[1]] = -halfHeight;
+		tmp[coneindices[2]] = 0.f;
+		return tmp;
+	}
+  }
+
+}
+
+SimdVector3	ConeShape::LocalGetSupportingVertexWithoutMargin(const SimdVector3& vec) const
+{
+		return ConeLocalSupport(vec);
+}
+
+SimdVector3	ConeShape::LocalGetSupportingVertex(const SimdVector3& vec)  const
+{
+	SimdVector3 supVertex = ConeLocalSupport(vec);
+	if ( GetMargin()!=0.f )
+	{
+		SimdVector3 vecnorm = vec;
+		if (vecnorm .length2() == 0.f)
+		{
+			vecnorm.setValue(-1.f,-1.f,-1.f);
+		} 
+		vecnorm.normalize();
+		supVertex+= GetMargin() * vecnorm;
+	}
+	return supVertex;
+}
+
+