void PhysXPhysics::VSyncVisibleScene()
{
	for (ActorIdToPysXRigidBodyTable::const_iterator it = m_actorRigidBodyMap.begin(); it != m_actorRigidBodyMap.end(); it++)
	{
		ActorId const id = it->first;

		PxTransform pxLoc = it->second->getGlobalPose();
		Mat4x4 loc;
		PxMatrixToMat4x4(PxMat44(pxLoc), &loc);
		
		Actor* pActor = g_pApp->m_pGame->VGetActor(id);
		if (pActor)
		{
			TransformComponent* pTransformComponent = pActor->GetComponent<TransformComponent>(TransformComponent::g_Name);

			if (pTransformComponent)
			{
				if (pTransformComponent->GetTransform() != loc)
				{
					Vec3 rot = loc.GetYawPitchRoll();
					pTransformComponent->SetPosition(loc.GetPosition());
					pTransformComponent->SetRotation(Vec3(XMConvertToDegrees(rot.x), XMConvertToDegrees(rot.y), XMConvertToDegrees(rot.z)));
					EventDataPtr pEvent(BE_NEW EvtData_Move_Actor(id, loc));
					IEventManager::Get()->VQueueEvent(pEvent);
				}
			}
		}
	}
}
void PhysXPhysics::AddShape(Actor* pActor, PxGeometry* geometry, float density, const std::string& physicsMaterial, bool gravityEnabled, float linearDamping, float angularDamping, const std::string& bodyType)
{
	BE_ASSERT(pActor);
	ActorId actorId = pActor->GetId();
	BE_ASSERTf(m_actorRigidBodyMap.find(actorId) == m_actorRigidBodyMap.end(), "Actor with more than one rigidbody");

	Mat4x4 transform = Mat4x4::g_Identity;
	
	TransformComponent* pTransformComponent = pActor->GetComponent<TransformComponent>(TransformComponent::g_Name);

	if (pTransformComponent)
	{
		transform = pTransformComponent->GetTransform();
	}
	else 
	{
		//Doesnt work without transform
		BE_ERROR("Actor %s PhysicsComponent requires Shape to have Transform Component: %d", actorId);
		return;
	}

	PhysicsMaterialData material(LookupMaterialData(physicsMaterial));
	PxMaterial* mat = m_pPhysicsSdk->createMaterial(material.m_friction, material.m_friction, material.m_restitution);

	Vec3 translation, scale;
	Quaternion rotation;
	
	bool ok = transform.Decompose(translation, rotation, scale);
	PxQuat pxRot;
	PxVec3 pxLoc;
	Vec3ToPxVec(translation, &pxLoc);
	QuaternionToPxQuat(rotation, &pxRot);
	PxTransform t(pxLoc, pxRot);

	if (bodyType == "Dynamic")
	{
		PxRigidDynamic* body = PxCreateDynamic(*m_pPhysicsSdk, t, *geometry, *mat, density);
		body->setActorFlag(PxActorFlag::eDISABLE_GRAVITY, !gravityEnabled);
		PxRigidBodyExt::updateMassAndInertia(*body, density);
		body->setLinearDamping(linearDamping);
		body->setAngularDamping(angularDamping);
		m_pScene->addActor(*body);

		m_actorRigidBodyMap[actorId] = body;
		m_rigidBodyActorMap[body] = actorId;
	}
	else
	{
		BE_ERROR("[Physics] BodyType not supported: %s", bodyType.c_str());
		return;
	}
}