void	btSoftRigidDynamicsWorld::rayTestSingle(const btTransform& rayFromTrans,const btTransform& rayToTrans,
					  btCollisionObject* collisionObject,
					  const btCollisionShape* collisionShape,
					  const btTransform& colObjWorldTransform,
					  RayResultCallback& resultCallback)
{
	if (collisionShape->isSoftBody()) {
		btSoftBody* softBody = btSoftBody::upcast(collisionObject);
		if (softBody) {
			btSoftBody::sRayCast softResult;
			if (softBody->rayTest(rayFromTrans.getOrigin(), rayToTrans.getOrigin(), softResult)) 
			{
				
				if (softResult.fraction<= resultCallback.m_closestHitFraction)
				{

					btCollisionWorld::LocalShapeInfo shapeInfo;
					shapeInfo.m_shapePart = 0;
					shapeInfo.m_triangleIndex = softResult.index;
					// get the normal
					btVector3 normal = softBody->m_faces[softResult.index].m_normal;
					btVector3 rayDir = rayToTrans.getOrigin() - rayFromTrans.getOrigin();
					if (normal.dot(rayDir) > 0) {
						// normal always point toward origin of the ray
						normal = -normal;
					}
					btCollisionWorld::LocalRayResult rayResult
						(collisionObject,
						 &shapeInfo,
						 normal,
						 softResult.fraction);
					bool	normalInWorldSpace = true;
					resultCallback.addSingleResult(rayResult,normalInWorldSpace);
				}
			}
		}
	} 
	else {
		btCollisionWorld::rayTestSingle(rayFromTrans,rayToTrans,collisionObject,collisionShape,colObjWorldTransform,resultCallback);
	}
}

void btContinuousConvexCollision::computeClosestPoints( const btTransform& transA, const btTransform& transB,btPointCollector& pointCollector)
{
	if (m_convexB1)
	{
		m_simplexSolver->reset();
		btGjkPairDetector gjk(m_convexA,m_convexB1,m_convexA->getShapeType(),m_convexB1->getShapeType(),m_convexA->getMargin(),m_convexB1->getMargin(),m_simplexSolver,m_penetrationDepthSolver);		
		btGjkPairDetector::ClosestPointInput input;
		input.m_transformA = transA;
		input.m_transformB = transB;
		gjk.getClosestPoints(input,pointCollector,0);
	} else
	{
		//convex versus plane
		const btConvexShape* convexShape = m_convexA;
		const btStaticPlaneShape* planeShape = m_planeShape;
		
		bool hasCollision = false;
		const btVector3& planeNormal = planeShape->getPlaneNormal();
		const btScalar& planeConstant = planeShape->getPlaneConstant();
		
		btTransform convexWorldTransform = transA;
		btTransform convexInPlaneTrans;
		convexInPlaneTrans= transB.inverse() * convexWorldTransform;
		btTransform planeInConvex;
		planeInConvex= convexWorldTransform.inverse() * transB;
		
		btVector3 vtx = convexShape->localGetSupportingVertex(planeInConvex.getBasis()*-planeNormal);

		btVector3 vtxInPlane = convexInPlaneTrans(vtx);
		btScalar distance = (planeNormal.dot(vtxInPlane) - planeConstant);

		btVector3 vtxInPlaneProjected = vtxInPlane - distance*planeNormal;
		btVector3 vtxInPlaneWorld = transB * vtxInPlaneProjected;
		btVector3 normalOnSurfaceB = transB.getBasis() * planeNormal;

		pointCollector.addContactPoint(
			normalOnSurfaceB,
			vtxInPlaneWorld,
			distance);
	}
}

bool parseTransform(btTransform& tr, TiXmlElement* xml, ErrorLogger* logger)
{
	tr.setIdentity();
	
	{
		const char* xyz_str = xml->Attribute("xyz");
		if (xyz_str)
		{
			parseVector3(tr.getOrigin(),std::string(xyz_str),logger);
		}
	}
	
	{
		const char* rpy_str = xml->Attribute("rpy");
		if (rpy_str != NULL)
		{
			btVector3 rpy;
			if (parseVector3(rpy,std::string(rpy_str),logger))
			{
				double phi, the, psi;
				double roll = rpy[0];
				double pitch = rpy[1];
				double yaw = rpy[2];
				
				phi = roll / 2.0;
				the = pitch / 2.0;
				psi = yaw / 2.0;
			  
				btQuaternion orn(
								sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
								cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
								cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
								 cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi));
			  
				orn.normalize();
				tr.setRotation(orn);
			}
		}
	}
	return true;
}

bool btGjkEpaPenetrationDepthSolver::calcPenDepth( btSimplexSolverInterface& simplexSolver,
											  const btConvexShape* pConvexA, const btConvexShape* pConvexB,
											  const btTransform& transformA, const btTransform& transformB,
											  btVector3& v, btVector3& wWitnessOnA, btVector3& wWitnessOnB,
											  class btIDebugDraw* debugDraw)
{

	(void)debugDraw;
	(void)v;
	(void)simplexSolver;

//	const btScalar				radialmargin(btScalar(0.));
	
	btVector3	guessVector(transformB.getOrigin()-transformA.getOrigin());
	btGjkEpaSolver2::sResults	results;
	

	if(btGjkEpaSolver2::Penetration(pConvexA,transformA,
								pConvexB,transformB,
								guessVector,results))
	
		{
	//	debugDraw->drawLine(results.witnesses[1],results.witnesses[1]+results.normal,btVector3(255,0,0));
		//resultOut->addContactPoint(results.normal,results.witnesses[1],-results.depth);
		wWitnessOnA = results.witnesses[0];
		wWitnessOnB = results.witnesses[1];
		v = results.normal;
		return true;		
		} else
	{
		if(btGjkEpaSolver2::Distance(pConvexA,transformA,pConvexB,transformB,guessVector,results))
		{
			wWitnessOnA = results.witnesses[0];
			wWitnessOnB = results.witnesses[1];
			v = results.normal;
			return false;
		}
	}

	return false;
}

		void TransformMotionState::getWorldTransform(btTransform& worldTrans) const {
			Matrix4x4 xformMatrix = toMatrix();

			float xformMatrixVals[16] = {
				xformMatrix[0], xformMatrix[1], xformMatrix[2], xformMatrix[3],
				xformMatrix[4], xformMatrix[5], xformMatrix[6], xformMatrix[7],
				xformMatrix[8], xformMatrix[9], xformMatrix[10], xformMatrix[11],
				xformMatrix[12], xformMatrix[13], xformMatrix[14], xformMatrix[15],
			};

			worldTrans.setFromOpenGLMatrix(xformMatrixVals);
		}

void Wheel::Update(float dt,btTransform & parentTransform)
{
	///ROTATION
	//get the quaternion rotation from euler angles using bt
	//offset rotation(initial rotation)
	btQuaternion btLocalRot = btQuaternion(m_yaw, m_pitch, m_roll);
	XMMATRIX localRot = XMMatrixRotationQuaternion(XMLoadFloat4(&XMFLOAT4(btLocalRot.getX(), btLocalRot.getY(), btLocalRot.getZ(), btLocalRot.getW())));
	//worldspace rotation
	btQuaternion btParentRot = parentTransform.getRotation();
	XMMATRIX ParentRot = XMMatrixRotationQuaternion(XMLoadFloat4(&XMFLOAT4(btParentRot.getX(), btParentRot.getY(), btParentRot.getZ(), btParentRot.getW())));
	//add rotations together
	XMMATRIX finalRot = localRot *  ParentRot;
	
	
	btVector3 btparentPos = parentTransform.getOrigin();
	XMMATRIX translationMat = XMMatrixTranslation(btparentPos.getX(), btparentPos.getY(), btparentPos.getZ());
	//XMMATRIX scaleMat = XMMatrixScaling(m_scale, m_scale, m_scale);
	XMMATRIX scaleMat = XMMatrixScaling(m_scale, m_scale, m_scale);
	XMMATRIX worldMat = finalRot * scaleMat * translationMat;
	XMStoreFloat4x4(&m_worldMat, worldMat);
}

void Matrix4_to_btTransform(JNIEnv * const &jenv, btTransform &target, jobject &source)
{
	matrix4_ensurefields(jenv, source);
	
	jfloatArray valArray = (jfloatArray) jenv->GetObjectField(source, matrix4_val);
	jfloat * elements = jenv->GetFloatArrayElements(valArray, NULL);
	
	target.setFromOpenGLMatrix(elements);
	
	jenv->ReleaseFloatArrayElements(valArray, elements, JNI_ABORT);
	jenv->DeleteLocalRef(valArray);
}

void MotionState::setWorldTransform(const btTransform& trans)
{
	if(!rigidbody->IsEnabled()) return;

	dvec3 pos = vec3_cast<dvec3>(trans.getOrigin());
	quat rot = quat_cast<quat>(trans.getRotation());

//	vec3 oldPos = rigidbody->owner->transform->GetWorldPosition();
//	quat oldRot = rigidbody->owner->transform->GetRotationQuat();

//	if(pos != oldPos)
	{
		rigidbody->owner->transform->SetWorldPosition(pos);
	}

//	if(rot != oldRot)
	rigidbody->owner->transform->SetRotationQuat(rot);

//	rigidbody->body->getWorldTransform().setOrigin(vec3_cast<btVector3>(pos));
//	rigidbody->body->getWorldTransform().setRotation(quat_cast<btQuaternion>(rigidbody->owner->transform->GetRotationQuat()));
}

void btTransform_to_Matrix4(JNIEnv * const &jenv, jobject &target, const btTransform &source)
{
	matrix4_ensurefields(jenv, target);
	
	jfloatArray valArray = (jfloatArray) jenv->GetObjectField(target, matrix4_val);
	jfloat * elements = jenv->GetFloatArrayElements(valArray, NULL);

	source.getOpenGLMatrix(elements);
	
	jenv->ReleaseFloatArrayElements(valArray, elements, 0);
	jenv->DeleteLocalRef(valArray);
}

	virtual void setWorldTransform(const btTransform& tform)
	{
		auto inv = glm::inverse(m_object->getRotation());
		const auto& rot = tform.getRotation();
		auto r2 = inv * glm::quat(rot.w(), rot.x(), rot.y(), rot.z());

		auto skeleton = m_object->skeleton;
		auto& prev = skeleton->getData(m_part->dummy->getIndex()).a;
		auto next = prev;
		next.rotation = r2;
		skeleton->setData(m_part->dummy->getIndex(), { next, prev, true } );
	}

//  draw sphere
void DebugDrawer::drawSphere(btScalar radius, const btTransform& transform, const btVector3& color, int half)
{
	btVector3 p = transform.getOrigin();
	const btMatrix3x3& m = transform.getBasis();

	const btVector3 x = m * btVector3(radius,0,0);
	const btVector3 y = m * btVector3(0,radius,0);
	const btVector3 z = m * btVector3(0,0,radius);

	const float PI2 = 2*M_PI, ad = M_PI/15.f;
	{
		btVector3 p1, p1o = p + y, p2o = p + x, p3o = p + y;
		for (float a = ad; a <= PI2+ad; a += ad)
		{
			float s = sinf(a), c = cosf(a);
			p1 = p - s*x + c*y;  drawLine(p1o, p1, color);	p1o = p1;
			p1 = p - s*z + c*x;  drawLine(p2o, p1, color);	p2o = p1;
			p1 = p - s*z + c*y;  drawLine(p3o, p1, color);	p3o = p1;
		}
	}
}

void CGmObjPhysMan::CMotionState::setWorldTransform( const btTransform &roWorldTransform )
{
	btDefaultMotionState::setWorldTransform( roWorldTransform );
	
	if( m_poShape )
	{
		//const btMatrix3x3 &roBasis = roWorldTransform.getBasis();
		//m_poShape->m_oMatrix.Unit();
		roWorldTransform.getOpenGLMatrix( m_poShape->m_oMatrix );
		//m_poShape->m_oMatrix.Translate( BT2TVEC3( roWorldTransform.getOrigin() ) );
	}
}

void DynamicsMotionState::getWorldTransform(btTransform& transform) const
{
//  DALI_LOG_INFO(Debug::Filter::gDynamics, Debug::Verbose, "%s\n", __PRETTY_FUNCTION__);

  // get node's world position and rotation
  Vector3 position;
  Quaternion rotation;
  Vector3 axis;
  float angle( 0.0f );

  mDynamicsBody.GetNodePositionAndRotation(position, rotation);
  rotation.ToAxisAngle( axis, angle );

  // modify parameters
  transform.setIdentity();
  transform.setOrigin(btVector3(position.x, position.y, position.z));
  if( axis != Vector3::ZERO )
  {
    transform.setRotation( btQuaternion(btVector3(axis.x, axis.y, axis.z), btScalar(angle)) );
  }
}

	btSingleSweepCallback(const btConvexShape* castShape, const btTransform& convexFromTrans,const btTransform& convexToTrans,const btCollisionWorld* world,btCollisionWorld::ConvexResultCallback& resultCallback,btScalar allowedPenetration)
		:m_convexFromTrans(convexFromTrans),
		m_convexToTrans(convexToTrans),
		m_world(world),
		m_resultCallback(resultCallback),
		m_allowedCcdPenetration(allowedPenetration),
		m_castShape(castShape)
	{
		btVector3 unnormalizedRayDir = (m_convexToTrans.getOrigin()-m_convexFromTrans.getOrigin());
		btVector3 rayDir = unnormalizedRayDir.normalized();
		///what about division by zero? --> just set rayDirection[i] to INF/1e30
		m_rayDirectionInverse[0] = rayDir[0] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[0];
		m_rayDirectionInverse[1] = rayDir[1] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[1];
		m_rayDirectionInverse[2] = rayDir[2] == btScalar(0.0) ? btScalar(1e30) : btScalar(1.0) / rayDir[2];
		m_signs[0] = m_rayDirectionInverse[0] < 0.0;
		m_signs[1] = m_rayDirectionInverse[1] < 0.0;
		m_signs[2] = m_rayDirectionInverse[2] < 0.0;

		m_lambda_max = rayDir.dot(unnormalizedRayDir);

	}

void LoadMeshFromColladaAssimp(const char* relativeFileName, btAlignedObjectArray<GLInstanceGraphicsShape>& visualShapes, btAlignedObjectArray<ColladaGraphicsInstance>& visualShapeInstances, btTransform& upAxisTrans, float& unitMeterScaling)
{
	upAxisTrans.setIdentity();
	unitMeterScaling = 1;

	GLInstanceGraphicsShape* shape = 0;

	FILE* file = fopen(relativeFileName, "rb");
	if (file)
	{
		int size = 0;
		if (fseek(file, 0, SEEK_END) || (size = ftell(file)) == EOF || fseek(file, 0, SEEK_SET))
		{
			b3Warning("Error: Cannot access file to determine size of %s\n", relativeFileName);
		}
		else
		{
			if (size)
			{
				//printf("Open DAE file of %d bytes\n",size);

				Assimp::Importer importer;
				//importer.SetPropertyInteger(AI_CONFIG_PP_RVC_FLAGS, aiComponent_NORMALS | aiComponent_COLORS);
				importer.SetPropertyInteger(AI_CONFIG_PP_SBP_REMOVE, aiPrimitiveType_LINE | aiPrimitiveType_POINT);
				//	importer.SetPropertyInteger(AI_CONFIG_IMPORT_COLLADA_IGNORE_UP_DIRECTION, 1);
				aiScene const* scene = importer.ReadFile(relativeFileName,
														 aiProcess_JoinIdenticalVertices |
															 //aiProcess_RemoveComponent |
															 aiProcess_SortByPType |
															 aiProcess_Triangulate);
				if (scene)
				{
					shape = &visualShapes.expand();
					shape->m_scaling[0] = 1;
					shape->m_scaling[1] = 1;
					shape->m_scaling[2] = 1;
					shape->m_scaling[3] = 1;
					int index = 0;
					shape->m_indices = new b3AlignedObjectArray<int>();
					shape->m_vertices = new b3AlignedObjectArray<GLInstanceVertex>();

					aiMatrix4x4 ident;
					addMeshParts(scene, scene->mRootNode, shape, ident);
					shape->m_numIndices = shape->m_indices->size();
					shape->m_numvertices = shape->m_vertices->size();
					ColladaGraphicsInstance& instance = visualShapeInstances.expand();
					instance.m_shapeIndex = visualShapes.size() - 1;
				}
			}
		}
	}
}

OSG::Matrix VRPhysics::fromBTTransform(const btTransform t) {
    btScalar _m[16];
    t.getOpenGLMatrix(_m);

    OSG::Matrix m;
    for (int i=0;i<4;i++) m[0][i] = _m[i];
    for (int i=0;i<4;i++) m[1][i] = _m[4+i];
    for (int i=0;i<4;i++) m[2][i] = _m[8+i];
    for (int i=0;i<4;i++) m[3][i] = _m[12+i];

    //cout << "fromTransform " << m << endl;
    return m;
}

D3DXMATRIX BulletPhysics::ConvertBTMatrix( btTransform &trn ) 
{
	//do crazy math shit
	btVector3 R = trn.getBasis().getColumn(0);
	btVector3 U = trn.getBasis().getColumn(1);
	btVector3 L = trn.getBasis().getColumn(2);
	btVector3 P = trn.getOrigin();

	D3DXVECTOR3 vR, vU, vL, vP;
	vR.x = R.x();vR.y = R.y();vR.z = R.z();
	vU.x = U.x();vU.y = U.y();vU.z = U.z();
	vL.x = L.x();vL.y = L.y();vL.z = L.z();
	vP.x = P.x();vP.y = P.y();vP.z = P.z();

	D3DXMATRIX matOutput;
	matOutput._11 = vR.x;matOutput._12 = vR.y;matOutput._13 = vR.z;matOutput._14 = 0.f;
	matOutput._21 = vU.x;matOutput._22 = vU.y;matOutput._23 = vU.z;matOutput._24 = 0.f;
	matOutput._31 = vL.x;matOutput._32 = vL.y;matOutput._33 = vL.z;matOutput._34 = 0.f;
	matOutput._41 = vP.x;matOutput._42 = vP.y;matOutput._43 = vP.z;matOutput._44 = 1.f;

	return matOutput;
}

void BeGraphicsModel::drawToDepth( const btTransform& transform )
{
// 	btScalar m[16];
	transform.getOpenGLMatrix(m_matrix);
	m_system->matrixPush(GL_MODELVIEW);
	m_system->matrixMult(GL_MODELVIEW, m_matrix);
	drawToDepth();

	m_system->matrixPop(GL_MODELVIEW);

	//glEnable(GL_CULL_FACE);
	//glCullFace(GL_BACK);
}

void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
{
	const btMatrix3x3& rot = tr.getBasis();
	const btVector3& r0 = rot[0];
	const btVector3& r1 = rot[1];
	const btVector3& r2 = rot[2];

	const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
	const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
	const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();

	out.setValue(x, y, z);
}

// This callback is invoked by the physics simulation at the end of each simulation step...
// iff the corresponding RigidBody is DYNAMIC and has moved.
void EntityMotionState::setWorldTransform(const btTransform& worldTrans) {
    if (!_entity) {
        return;
    }

    assert(entityTreeIsLocked());
    measureBodyAcceleration();
    bool positionSuccess;
    _entity->setPosition(bulletToGLM(worldTrans.getOrigin()) + ObjectMotionState::getWorldOffset(), positionSuccess, false);
    if (!positionSuccess) {
        qDebug() << "EntityMotionState::setWorldTransform setPosition failed" << _entity->getID();
    }
    bool orientationSuccess;
    _entity->setOrientation(bulletToGLM(worldTrans.getRotation()), orientationSuccess, false);
    if (!orientationSuccess) {
        qDebug() << "EntityMotionState::setWorldTransform setOrientation failed" << _entity->getID();
    }
    _entity->setVelocity(getBodyLinearVelocity());
    _entity->setAngularVelocity(getBodyAngularVelocity());
    _entity->setLastSimulated(usecTimestampNow());

    if (_entity->getSimulatorID().isNull()) {
        _loopsWithoutOwner++;
        if (_loopsWithoutOwner > LOOPS_FOR_SIMULATION_ORPHAN && usecTimestampNow() > _nextOwnershipBid) {
            upgradeOutgoingPriority(VOLUNTEER_SIMULATION_PRIORITY);
        }
    }

    #ifdef WANT_DEBUG
        quint64 now = usecTimestampNow();
        qCDebug(physics) << "EntityMotionState::setWorldTransform()... changed entity:" << _entity->getEntityItemID();
        qCDebug(physics) << "       last edited:" << _entity->getLastEdited()
                         << formatUsecTime(now - _entity->getLastEdited()) << "ago";
        qCDebug(physics) << "    last simulated:" << _entity->getLastSimulated()
                         << formatUsecTime(now - _entity->getLastSimulated()) << "ago";
        qCDebug(physics) << "      last updated:" << _entity->getLastUpdated()
                         << formatUsecTime(now - _entity->getLastUpdated()) << "ago";
    #endif
}