/* Copyright (c) <2003-2011> <Julio Jerez, Newton Game Dynamics>
* 
* This software is provided 'as-is', without any express or implied
* warranty. In no event will the authors be held liable for any damages
* arising from the use of this software.
* 
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 
* 3. This notice may not be removed or altered from any source distribution.
*/

#include "dgPhysicsStdafx.h"
#include "dgBody.h"
#include "dgContact.h"
#include "dgCollisionCylinder.h"

//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////


dgInt32 dgCollisionCylinder::m_shapeRefCount = 0;
dgConvexSimplexEdge dgCollisionCylinder::m_edgeArray[DG_CYLINDER_SEGMENTS * 2 * 3];

dgCollisionCylinder::dgCollisionCylinder(
	dgMemoryAllocator* allocator, 
	dgUnsigned32 signature, 
	dgFloat32 radius, 
	dgFloat32 height, 
	const dgMatrix& matrix)
	:dgCollisionConvex(allocator, signature, matrix, m_cylinderCollision)
{
	Init (radius, height);
}


dgCollisionCylinder::dgCollisionCylinder(dgWorld* const world, dgDeserialize deserialization, void* const userData)
	:dgCollisionConvex (world, deserialization, userData)
{
	dgVector size;
	deserialization (userData, &size, sizeof (dgVector));
	Init (size.m_x, size.m_y);
}

void dgCollisionCylinder::Init (dgFloat32 radius, dgFloat32 height)
{
//	dgInt32 i;
//	dgInt32 j;
//	dgEdge *edge;
//	dgFloat32 y;
//	dgFloat32 z;
//	dgFloat32 angle;

	m_rtti |= dgCollisionCylinder_RTTI;
	m_radius = dgAbsf (radius);
	m_height[0] = dgAbsf (height * dgFloat32 (0.5f));
	m_height[1] = -m_height[0];

	dgFloat32 angle = dgFloat32 (0.0f);
	for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) {
		dgFloat32 z = dgSin (angle) * m_radius;
		dgFloat32 y = dgCos (angle) * m_radius;
		m_vertex[i                       ] = dgVector (- m_height[0], y, z, dgFloat32 (1.0f));
		m_vertex[i + DG_CYLINDER_SEGMENTS] = dgVector (  m_height[0], y, z, dgFloat32 (1.0f));
		angle += dgPI2 / DG_CYLINDER_SEGMENTS;
	}

	m_edgeCount = DG_CYLINDER_SEGMENTS * 6;
	m_vertexCount = DG_CYLINDER_SEGMENTS * 2;
	dgCollisionConvex::m_vertex = m_vertex;

	if (!m_shapeRefCount) {
		dgPolyhedra polyhedra(m_allocator);
		dgInt32 wireframe[DG_CYLINDER_SEGMENTS];

		dgInt32 j = DG_CYLINDER_SEGMENTS - 1;
		polyhedra.BeginFace ();
		for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) { 
			wireframe[0] = j;
			wireframe[1] = i;
			wireframe[2] = i + DG_CYLINDER_SEGMENTS;
			wireframe[3] = j + DG_CYLINDER_SEGMENTS;
			j = i;
			polyhedra.AddFace (4, wireframe);
		}

		for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) { 
			wireframe[i] = DG_CYLINDER_SEGMENTS - 1 - i;
		}
		polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);

		for (dgInt32 i = 0; i < DG_CYLINDER_SEGMENTS; i ++) { 
			wireframe[i] = i + DG_CYLINDER_SEGMENTS;
		}
		polyhedra.AddFace (DG_CYLINDER_SEGMENTS, wireframe);
		polyhedra.EndFace ();

		_ASSERTE (SanityCheck (polyhedra));

		dgUnsigned64 i = 0;
		dgPolyhedra::Iterator iter (polyhedra);
		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &(*iter);
			edge->m_userData = i;
			i ++;
		}

		for (iter.Begin(); iter; iter ++) {
			dgEdge* const edge = &(*iter);

			dgConvexSimplexEdge* const ptr = &m_edgeArray[edge->m_userData];
			ptr->m_vertex = edge->m_incidentVertex;
			ptr->m_next = &m_edgeArray[edge->m_next->m_userData];
			ptr->m_prev = &m_edgeArray[edge->m_prev->m_userData];
			ptr->m_twin = &m_edgeArray[edge->m_twin->m_userData];
		}
	}

	m_shapeRefCount ++;
	dgCollisionConvex::m_simplex = m_edgeArray;

	SetVolumeAndCG ();
}

dgCollisionCylinder::~dgCollisionCylinder()
{
	m_shapeRefCount --;
	_ASSERTE (m_shapeRefCount >= 0);

	dgCollisionConvex::m_simplex = NULL;
	dgCollisionConvex::m_vertex = NULL;
}

dgInt32 dgCollisionCylinder::CalculateSignature () const
{
	dgUnsigned32 buffer[2 * sizeof (dgMatrix) / sizeof(dgInt32)];

	memset (buffer, 0, sizeof (buffer));
	buffer[0] = m_cylinderCollision;
	buffer[1] = dgCollision::Quantize (m_radius);
	buffer[2] = dgCollision::Quantize (m_height[0]);
	memcpy (&buffer[3], &m_offset, sizeof (dgMatrix));
	return dgInt32 (dgCollision::MakeCRC(buffer, sizeof (buffer)));
}



void dgCollisionCylinder::SetCollisionBBox (const dgVector& p0__, const dgVector& p1__)
{
	_ASSERTE (0);
}


void dgCollisionCylinder::DebugCollision (const dgMatrix& matrixPtr, OnDebugCollisionMeshCallback callback, void* const userData) const
{
	dgInt32 i;
	dgInt32 j;
	dgFloat32 y;
	dgFloat32 z;
	dgFloat32 angle;
	dgTriplex pool[24 * 2];

	angle = dgFloat32 (0.0f);
	for (i = 0; i < 24; i ++) {
		z = dgSin (angle) * m_radius;
		y = dgCos (angle) * m_radius;
		pool[i].m_x = - m_height[0];
		pool[i].m_y = y;
		pool[i].m_z = z;
		pool[i + 24].m_x = m_height[0];
		pool[i + 24].m_y = y;
		pool[i + 24].m_z = z;
		angle += dgPI2 / dgFloat32 (24.0f);
	}


	dgMatrix matrix (GetOffsetMatrix() * matrixPtr);
	matrix.TransformTriplex (pool, sizeof (dgTriplex), pool, sizeof (dgTriplex), 24 * 2);

	dgTriplex face[24];

	j = 24 - 1;
	for (i = 0; i < 24; i ++) { 
		face[0] = pool[j];
		face[1] = pool[i];
		face[2] = pool[i + 24];
		face[3] = pool[j + 24];
		j = i;
		callback (userData, 4, &face[0].m_x, 0);
	}

	for (i = 0; i < 24; i ++) { 
		face[i] = pool[24 - 1 - i];
	}
	callback (userData, 24, &face[0].m_x, 0);
		
	for (i = 0; i < 24; i ++) { 
		face[i] = pool[i + 24];
	}
	callback (userData, 24, &face[0].m_x, 0);
}


dgVector dgCollisionCylinder::SupportVertexSimd (const dgVector& dir) const
{
	return SupportVertex (dir);
}



dgVector dgCollisionCylinder::SupportVertex (const dgVector& dir) const
{
/*
	dgInt32 index;
	dgFloat32 y0;
	dgFloat32 z0;
	dgFloat32 y1;
	dgFloat32 z1;
	dgFloat32 dist0;
	dgFloat32 dist1;

	_ASSERTE (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));

	//	sign = dir.m_x > dgFloat32 (0.0f) ? dgFloat32 (1.0f) : -dgFloat32 (1.0f);
	dgFloatSign *ptr = (dgFloatSign*) &dir; 
	index = -(ptr[0].m_integer.m_iVal >> 31);
	dgSinCos (m_tethaStep * dgFloor (dgAtan2 (dir.m_y, dir.m_z) * m_tethaStepInv), y0, z0);

	y0 *= m_radius;
	z0 *= m_radius;

	y1 = y0 * m_delCosTetha + z0 * m_delSinTetha;
	z1 = z0 * m_delCosTetha - y0 * m_delSinTetha;

	dist0 = dir.m_y * y0 + dir.m_z * z0;
	dist1 = dir.m_y * y1 + dir.m_z * z1;
	if (dist1 > dist0) {
		y0 = y1;
		z0 = z1;
	}
	return dgVector (m_height[index], y0, z0, dgFloat32 (0.0f));       
*/

	
	dgInt32 index;
	dgFloat32 y0;
	dgFloat32 z0;
	dgFloat32 mag2;
	dgFloatSign const *ptr = (dgFloatSign*) &dir; 

	_ASSERTE (dgAbsf ((dir % dir - dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));

	y0 = m_radius;
	z0 = dgFloat32 (0.0f);
	mag2 = dir.m_y * dir.m_y + dir.m_z * dir.m_z;
	if (mag2 > dgFloat32 (1.0e-12f)) {
		mag2 = dgRsqrt(mag2);
		y0 = dir.m_y * m_radius * mag2;
		z0 = dir.m_z * m_radius * mag2;
	}
	index = -(ptr[0].m_integer.m_iVal >> 31);

	return dgVector (m_height[index], y0, z0, dgFloat32 (0.0f));       
}

/*
dgVector dgCollisionCylinder::ImplicitCylindexSupport (const dgVector& dir) const
{
	dgFloat32 sign;
	dgFloat32 invMag;

	_ASSERTE ((dir % dir - dgFloat32 (dgFloat32 (1.0f))) < dgFloat32 (1.0e-3f));
	sign = dir.m_x > dgFloat32 (0.0f) ? dgFloat32 (dgFloat32 (1.0f)) : -dgFloat32 (dgFloat32 (1.0f));
	
	invMag = m_radius * dgRsqrt (dir.m_y * dir.m_y + dir.m_z * dir.m_z + 1.0e-12f) ;
	return dgVector (m_height[0] * sign, invMag * dir.m_y, invMag * dir.m_z, dgFloat32 (0.0f));       
}
*/


dgFloat32 dgCollisionCylinder::CalculateMassProperties (dgVector& inertia, dgVector& crossInertia, dgVector& centerOfMass) const
{
	dgFloat32 volume;
	dgFloat32 inertaxx;
	dgFloat32 inertayyzz;

//volume = dgCollisionConvex::CalculateMassProperties (inertia, crossInertia, centerOfMass);


	centerOfMass = GetOffsetMatrix().m_posit;
	volume = dgFloat32 (3.1616f * 2.0f) * m_radius * m_radius * m_height[0]; 

	inertaxx   = dgFloat32 (0.5f) * m_radius *  m_radius * volume;
	inertayyzz = (dgFloat32 (0.25f) * m_radius *  m_radius + dgFloat32 (1.0f / 3.0f) * m_height[0] * m_height[0]) * volume;

	dgMatrix inertiaTensor (dgGetIdentityMatrix());

	inertiaTensor[0][0] = inertaxx;
	inertiaTensor[1][1] = inertayyzz;
	inertiaTensor[2][2] = inertayyzz;

	inertiaTensor = GetOffsetMatrix().Inverse() * inertiaTensor * GetOffsetMatrix();

	crossInertia.m_x = inertiaTensor[1][2] - volume * centerOfMass.m_y * centerOfMass.m_z;
	crossInertia.m_y = inertiaTensor[0][2] - volume * centerOfMass.m_z * centerOfMass.m_x;
	crossInertia.m_z = inertiaTensor[0][1] - volume * centerOfMass.m_x * centerOfMass.m_y;

	dgVector central (centerOfMass.CompProduct(centerOfMass));
	inertia.m_x = inertiaTensor[0][0] + volume * (central.m_y + central.m_z);
	inertia.m_y = inertiaTensor[1][1] + volume * (central.m_z + central.m_x);
	inertia.m_z = inertiaTensor[2][2] + volume * (central.m_x + central.m_y);

	centerOfMass = centerOfMass.Scale (volume);
	return volume;
}


dgInt32 dgCollisionCylinder::CalculatePlaneIntersection (
	const dgVector& normal, 
	const dgVector& origin, 
	dgVector contactsOut[]) const
{
	dgInt32 i;
	dgInt32 count;
	dgFloat32 y;
	dgFloat32 z;
	dgFloat32 cosAng;
	dgFloat32 sinAng;
	dgFloat32 magInv;

	if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) { 
		magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
		cosAng = normal.m_y * magInv;
		sinAng = normal.m_z * magInv;

//		dgMatrix matrix (dgGetIdentityMatrix ());
//		matrix[1][1] = cosAng;
//		matrix[1][2] = sinAng;
//		matrix[2][1] = -sinAng;
//		matrix[2][2] = cosAng;
//		dgVector normal1 (matrix.UnrotateVector (normal));
//		dgVector origin1 (matrix.UnrotateVector (origin));

		_ASSERTE (dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
//		dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, 
//									  normal.m_z * cosAng - normal.m_y * sinAng, dgFloat32 (0.0f));
		dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32 (0.0f), dgFloat32 (0.0f));
		dgVector origin1 (origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, 
									  origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32 (0.0f));

		count = dgCollisionConvex::CalculatePlaneIntersection (normal1, origin1, contactsOut);
//		matrix.TransformTriplex (contactsOut, sizeof (dgVector), contactsOut, sizeof (dgVector), count); 
		for (i = 0; i < count; i ++) {
			y = contactsOut[i].m_y;
			z = contactsOut[i].m_z;
			contactsOut[i].m_y = y * cosAng - z * sinAng; 
			contactsOut[i].m_z = z * cosAng + y * sinAng;
		}

	} else {
		count = dgCollisionConvex::CalculatePlaneIntersection (normal, origin, contactsOut);
	}
	return count;
}

dgInt32 dgCollisionCylinder::CalculatePlaneIntersectionSimd (
	const dgVector& normal, 
	const dgVector& origin, 
	dgVector contactsOut[]) const
{
#ifdef DG_BUILD_SIMD_CODE
	dgInt32 i;
	dgInt32 count;
	dgFloat32 y;
	dgFloat32 z;
	dgFloat32 cosAng;
	dgFloat32 sinAng;
	dgFloat32 magInv;
	simd_type tmp0;
	simd_type mag2;

	if (dgAbsf (normal.m_x) < dgFloat32 (0.999f)) { 
//		magInv = dgRsqrt (normal.m_y * normal.m_y + normal.m_z * normal.m_z);
		y = normal.m_y * normal.m_y + normal.m_z * normal.m_z;
		mag2  = simd_load_s (y);
		tmp0 = simd_rsqrt_s(mag2);
		simd_store_s (simd_mul_s (simd_mul_s(*(simd_type*)&m_nrh0p5, tmp0), simd_mul_sub_s (*(simd_type*)&m_nrh3p0, simd_mul_s (mag2, tmp0), tmp0)), &magInv);

		cosAng = normal.m_y * magInv;
		sinAng = normal.m_z * magInv;

//		dgMatrix matrix (dgGetIdentityMatrix ());
//		matrix[1][1] = cosAng;
//		matrix[1][2] = sinAng;
//		matrix[2][1] = -sinAng;
//		matrix[2][2] = cosAng;
//		dgVector normal1 (matrix.UnrotateVector (normal));
//		dgVector origin1 (matrix.UnrotateVector (origin));
		_ASSERTE (dgAbsf (normal.m_z * cosAng - normal.m_y * sinAng) < dgFloat32 (1.0e-4f));
//		dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, 
//									  normal.m_z * cosAng - normal.m_y * sinAng, dgFloat32 (0.0f));
		dgVector normal1 (normal.m_x, normal.m_y * cosAng + normal.m_z * sinAng, dgFloat32 (0.0f), dgFloat32 (0.0f));
		dgVector origin1 (origin.m_x, origin.m_y * cosAng + origin.m_z * sinAng, 
									  origin.m_z * cosAng - origin.m_y * sinAng, dgFloat32 (0.0f));

		count = dgCollisionConvex::CalculatePlaneIntersectionSimd (normal1, origin1, contactsOut);
//		matrix.TransformTriplex (contactsOut, sizeof (dgVector), contactsOut, sizeof (dgVector), count); 
		for (i = 0; i < count; i ++) {
			y = contactsOut[i].m_y;
			z = contactsOut[i].m_z;
			contactsOut[i].m_y = y * cosAng - z * sinAng; 
			contactsOut[i].m_z = z * cosAng + y * sinAng;
		}

	} else {
		count = dgCollisionConvex::CalculatePlaneIntersectionSimd (normal, origin, contactsOut);
	}

	return count;

#else
	return 0;
#endif
}


dgFloat32 dgCollisionCylinder::RayCast (const dgVector& q0, const dgVector& q1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
	dgFloat32 t;
	dgFloat32 a;
	dgFloat32 b;
	dgFloat32 c;
	dgFloat32 y;
	dgFloat32 z;
	dgFloat32 t1;
	dgFloat32 desc;

	if (PREFILTER_RAYCAST (preFilter, body, this, userData)) {
		return dgFloat32 (1.2f);
	}

	t = dgFloat32 (1.2f);
	dgVector p0 (q0);
	p0.m_x = dgFloat32 (0.0f);
	c = (p0 % p0) - m_radius * m_radius;
	if (c > dgFloat32 (0.0f)) {

		dgVector dp (q1 - q0);
		dp.m_x = dgFloat32 (0.0f);
		a = dp % dp;
		b = dgFloat32 (2.0f) * (p0 % dp);

		desc = b * b - 4.0f * a * c;
		if (desc > 1.0e-8f) {
			desc = dgSqrt (desc);
			a = dgFloat32 (1.0f) / (dgFloat32 (2.0f) * a);
			t1 = GetMin ((- b + desc) * a, (- b - desc) * a);
			if ((t1 < dgFloat32 (1.0f)) && (t1 >= dgFloat32 (0.0f))) {
				dgVector dq (q1 - q0);
				dgVector contact (q0 + dq.Scale (t1));
				if (contact.m_x > m_height[0]) {
					if (q1.m_x < m_height[0]) {
						t1 = (m_height[0] - q0.m_x) / (q1.m_x - q0.m_x);
						y = q0.m_y + (q1.m_y - q0.m_y) * t1;
						z = q0.m_z + (q1.m_z - q0.m_z) * t1;
						if ((y * y + z * z - m_radius * m_radius) < dgFloat32 (0.0f)) {
							t = t1;
							contactOut.m_normal = dgVector (dgFloat32 (dgFloat32 (1.0f)), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
							contactOut.m_userId = SetUserDataID();
						}
					}
				} else if (contact.m_x < -m_height[0]) {
					if (q1.m_x > -m_height[0]) {
						t1 = (-m_height[0] - q0.m_x) / (q1.m_x - q0.m_x);
						y = q0.m_y + (q1.m_y - q0.m_y) * t1;
						z = q0.m_z + (q1.m_z - q0.m_z) * t1;
						if ((y * y + z * z - m_radius * m_radius) < dgFloat32 (0.0f)) {
							t = t1;
							contactOut.m_normal = dgVector (-dgFloat32 (dgFloat32 (1.0f)), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
							contactOut.m_userId = SetUserDataID();
						}
					}
				} else if (t1 >=  dgFloat32 (0.0f)) {
					t = t1;
					dgVector n (contact);
					n.m_x = dgFloat32 (0.0f);
					contactOut.m_normal = n.Scale (dgRsqrt (n % n));
					contactOut.m_userId = SetUserDataID();
				}
			}
		}
	} else {
		if (q0.m_x > m_height[0]) {
			if (q1.m_x < m_height[0]) {
				t1 = (m_height[0] - q0.m_x) / (q1.m_x - q0.m_x);
				y = q0.m_y + (q1.m_y - q0.m_y) * t1;
				z = q0.m_z + (q1.m_z - q0.m_z) * t1;
				if ((y * y + z * z - m_radius * m_radius) < dgFloat32 (0.0f)) {
					t = t1;
					contactOut.m_normal = dgVector (dgFloat32 (dgFloat32 (1.0f)), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
					contactOut.m_userId = SetUserDataID();
				}
			}
		} else if (q0.m_x < -m_height[0]) {
			if (q1.m_x > -m_height[0]) {
				t1 = (-m_height[0] - q0.m_x) / (q1.m_x - q0.m_x);
				y = q0.m_y + (q1.m_y - q0.m_y) * t1;
				z = q0.m_z + (q1.m_z - q0.m_z) * t1;
				if ((y * y + z * z - m_radius * m_radius) < dgFloat32 (0.0f)) {
					t = t1;
					contactOut.m_normal = dgVector (-dgFloat32 (dgFloat32 (1.0f)), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
					contactOut.m_userId = SetUserDataID();
				}
			}
		}
	}
	return t;
}

dgFloat32 dgCollisionCylinder::RayCastSimd (const dgVector& q0, const dgVector& q1, dgContactPoint& contactOut, OnRayPrecastAction preFilter, const dgBody* const body, void* const userData) const
{
	return RayCast (q0, q1, contactOut, preFilter, body, userData);
}


void dgCollisionCylinder::GetCollisionInfo(dgCollisionInfo* info) const
{
	dgCollisionConvex::GetCollisionInfo(info);

	info->m_cylinder.m_r0 = m_radius;
	info->m_cylinder.m_r1 = m_radius;
	info->m_cylinder.m_height = m_height[0] * dgFloat32 (2.0f);
	info->m_offsetMatrix = GetOffsetMatrix();
//	strcpy (info->m_collisionType, "cylinder");
	info->m_collisionType = m_collsionId;
}


void dgCollisionCylinder::Serialize(dgSerialize callback, void* const userData) const
{
	dgVector size (dgAbsf (m_radius), m_height[0] * dgFloat32 (2.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
	SerializeLow(callback, userData);
	callback (userData, &size, sizeof (dgVector));
}