void CollisionFace::SphereCollision(const VC3 &position, float radius, Storm3D_CollisionInfo &colinfo, bool accurate)
{
	VC3 closest;
	getClosestPoint(position, vertex0, vertex1, vertex2, closest);

	float sqRange = closest.GetSquareRangeTo(position);
	if(sqRange < radius * radius && sqRange < colinfo.range * colinfo.range)
	{
		colinfo.hit = true;
		colinfo.range = sqrtf(sqRange);
		colinfo.position = closest;
		colinfo.plane_normal = plane.planenormal;
		colinfo.inside_amount = radius - colinfo.range;

		float planeRange = plane.GetPointRange(position);
		if(planeRange < 0)
			colinfo.plane_normal = -colinfo.plane_normal;
	}
}
float SpotLightCalculator::getLightAmount(const VC3 &position, const IStorm3D_Terrain &terrain, float rayHeight) const
{
	float squareRange = position.GetSquareRangeTo(data->position);
	if(squareRange > data->squareRange)
		return 0;

	VC2 pdir(position.x - data->position.x, position.z - data->position.z);
	VC2 sdir(data->direction.x, data->direction.z);

	float pAngle = pdir.CalculateAngle();
	float sAngle = sdir.CalculateAngle();
	float diff = pAngle - sAngle;
	//assert(diff

	if(diff < -3.1415f)
		diff += 3.1415f * 2;
	else if(diff > 3.1415f)
		diff -= 3.1415f * 2;

	if(fabsf(diff) / 3.1415f * 180.f > data->fov)
		return 0;

	return data->getAmount(position, -diff, terrain, rayHeight);
}
void ParticlePhysics::update()
{
	if(!data->physics)
		return;

	data->actorBaseList.clear();
	data->fluidBaseList.clear();

	// Update physics actors
	{
		for(PhysicsActorList::iterator it = data->physicsActorList.begin(); it != data->physicsActorList.end(); ++it)
		{
			PhysicsActor *actor = *it;
			if(!actor->actor)
				continue;

			/*
			if(actor->actor)
			{
				VC3 velocity;
				actor->actor->getVelocity(velocity);

				float len = velocity.GetLength();
				if(len > 1)
				{
					velocity /= len;
					actor->actor->setVelocity(velocity);
				}
			}
			*/

#ifndef PHYSX_GRAVITY
			if(actor->force.GetSquareLength() > 0.01f)
			{
				if(!actor->forceUpdate && ++actor->sleepCounter % 10 == 0)
				{
					VC3 currentPos;
					actor->getPosition(currentPos);
					VC3 currentAngular;
					actor->actor->getAngularVelocity(currentAngular);

					if(
						currentPos.GetSquareRangeTo(actor->lastPosition1) < 0.00001f
						||
						currentAngular.GetSquareRangeTo(actor->lastAngular1) < 0.00001f
						||
						currentPos.GetSquareRangeTo(actor->lastPosition2) < 0.00001f
						||
						currentAngular.GetSquareRangeTo(actor->lastAngular2) < 0.00001f)
					{
						if(!actor->actor->isSleeping())
							actor->actor->putToSleep();
					}

					actor->lastPosition2 = actor->lastPosition1;
					actor->lastAngular2 = actor->lastAngular1;

					actor->lastPosition1 = currentPos;
					actor->lastAngular1 = currentAngular;
				}
	
				if(actor->forceUpdate || !actor->actor->isSleeping())
					actor->actor->addVelocityChange(actor->force);

				actor->force = VC3();
			}
#else
			if(actor->forceUpdate)
			{
				VC3 base;
				actor->actor->getVelocity(base);

				VC3 newForce = base;
				newForce += actor->force;

				float len = newForce.GetSquareLength();
				if(len > MAX_VELOCITY * MAX_VELOCITY)
				{
					len = sqrtf(len);
					newForce /= len;

					newForce *= MAX_VELOCITY;
				}

				newForce -= base;
				actor->actor->addVelocityChange(newForce);

				{
					QUAT rot;
					rotateToward(VC3(0, 1.f, 0), actor->force.GetNormalized(), rot);

					VC3 test(rot.x, rot.y, rot.z);
					test.x += ((float)(rand() - RAND_MAX/2) / (float)RAND_MAX/2);
					test.z += ((float)(rand() - RAND_MAX/2) / (float)RAND_MAX/2);

					if(test.GetSquareLength() > 0.001f)
						test.Normalize();

					test *= 2.f;
					actor->actor->setAngularVelocity(test * 2.f);
				}

				actor->force = VC3();
			}
#endif

			actor->forceUpdate = false;
		}

		physics::resetFluidParticleCount();
		for(FluidActorList::iterator it = data->fluidActorList.begin(); it != data->fluidActorList.end(); ++it)
		{
			PhysicsFluid *fluid = *it;
			if(!fluid->fluid)
				continue;

			if(fluid->addAmount)
			{
				fluid->fluid->addParticles(&fluid->buffer[0], fluid->addAmount);
				fluid->buffer.clear();
				fluid->addAmount = 0;
			}

			fluid->fluid->setAcceleration(fluid->acceleration);
			fluid->fluid->update();
			//fluid->renderFlag = false;
		}
	}

	/*
	// Meshes
	{
		for(MeshList::iterator it = data->meshList.begin(); it != data->meshList.end(); ++it)
		{
			MeshData &convex = *it;
			boost::shared_ptr<PhysicsMesh> mesh = convex.mesh.lock();
			if(!mesh)
				continue;

			filesystem::FB_FILE *fp = filesystem::fb_fopen(convex.filename.c_str(), "rb");
			if(!fp)
			{
				physics::Cooker cooker;
				cooker.cookApproxConvex(convex.filename.c_str(), convex.object);
			}
			else
				filesystem::fb_fclose(fp);

			mesh->mesh = data->physics->createConvexMesh(convex.filename.c_str());
		}

		data->meshList.clear();
	}
	*/

	// Actors
	{
		/*
		if(actorList.size() + physicsActorList.size() > MAX_ACTOR_AMOUNT)
		{
			boost::shared_ptr<PhysicsActor> nullActor;
			return nullActor;
		}
		*/

		bool sort = false;

		int createAmount = data->actorList.size();
		if(createAmount > data->maxParticleSpawnAmount)
		{
			createAmount = data->maxParticleSpawnAmount;
			sort = true;
		}
		if(createAmount + data->createdActors > data->maxParticleAmount)
		{
			sort = true;
			createAmount = data->maxParticleAmount - data->createdActors;
		}

		if(sort)
			std::sort(data->actorList.begin(), data->actorList.end(), ActorListSorter());

		// For collision test
#ifdef PHYSX_SPAWN_TEST
		std::vector<BoxStruct> previousBoxes;
#endif

		int index = 0;
		for(ActorList::iterator it = data->actorList.begin(); it != data->actorList.end(); ++it)
		{
			ActorData &convex = *it;
			if(convex.createDelayCounter++ >= MAX_WAIT_FOR_CREATE)
				continue;

			boost::shared_ptr<PhysicsActor> actor = convex.actor.lock();
			if(!actor)
				continue;

			boost::shared_ptr<PhysicsMesh> mesh = convex.mesh.lock();
			if(!mesh)
				continue;

			/*
			//VC3 boxCenter = mesh->localPosition + actor->position;
			VC3 boxCenter = mesh->localPosition;
			actor->rotation.RotateVector(boxCenter);
			boxCenter += actor->position;

			// Test to previous tick physics
			if(data->physics->checkOverlapOBB(boxCenter, mesh->size, actor->rotation, physics::PhysicsLib::CollisionStatic))
			{
				convex.actor.reset();
				convex.mesh.reset();
				continue;
			}
			*/

#ifdef PHYSX_SPAWN_TEST
			BoxStruct currentBox;
			currentBox.center.set(boxCenter.x, boxCenter.y, boxCenter.z);
			currentBox.extents.set(mesh->size.x, mesh->size.y, mesh->size.z);
			QUAT r = actor->rotation.GetInverse();
			NxQuat quat;
			quat.setXYZW(r.x, r.y, r.z, r.w);
			currentBox.rotation.fromQuat(quat);

			// Test to previous tick physics
			if(data->physics->checkOverlapOBB(boxCenter, mesh->size, actor->rotation))
				continue;

			bool hit = false;
			for(unsigned int i = 0; i < previousBoxes.size(); ++i)
			{
				const BoxStruct &b = previousBoxes[i];
				if(NxBoxBoxIntersect(currentBox.extents, currentBox.center, currentBox.rotation, b.extents, b.center, b.rotation, true))
				{
					hit = true;
					break;
				}
			}

			if(hit)
				continue;
#endif

			// Don't create too many particles
			if(index++ >= createAmount)
				break;

			/*
			boost::shared_ptr<physics::ConvexActor> convexActor = data->physics->createConvexActor(mesh->mesh, actor->position);
			if(convexActor)
			{
				convexActor->setRotation(actor->rotation);
				convexActor->setVelocity(convex.velocity);
				convexActor->setAngularVelocity(convex.angularVelocity);
				convexActor->setMass(convex.mass);
				convexActor->setCollisionGroup(2);
				//convexActor->enableFeature(physics::ActorBase::DISABLE_GRAVITY, true);
				actor->actor = convexActor;
			}
			*/

			boost::shared_ptr<physics::BoxActor> boxActor = data->physics->createBoxActor(mesh->size, actor->position, mesh->localPosition);
			if(boxActor)
			{
				++data->createdActors;

				boxActor->setRotation(actor->rotation);
				boxActor->setVelocity(convex.velocity);
				boxActor->setAngularVelocity(convex.angularVelocity);
				boxActor->setMass(convex.mass);
				boxActor->setCollisionGroup(convex.collisionGroup);

#ifndef PHYSX_GRAVITY
				boxActor->enableFeature(physics::ActorBase::DISABLE_GRAVITY, true);
#endif

				boxActor->setIntData(convex.soundGroup);
				actor->actor = boxActor;

#ifdef PHYSX_SPAWN_TEST
				previousBoxes.push_back(currentBox);
#endif
			}
		}

		int size = data->actorList.size();
		for(int i = index; i < size; ++i)
		{
			data->actorList[i].createDelayCounter++;
		}

		if(index < size)
		{
			for(int i = 0; i < size - index; ++i)
				data->actorList[i] = data->actorList[i + index];

			data->actorList.resize(size - index);
		}
		else 
			data->actorList.clear();
	}

	// Fluids
	{
		for(FluidList::iterator it = data->fluidList.begin(); it != data->fluidList.end(); ++it)
		{
			FluidData &fluidData = *it;
			boost::shared_ptr<PhysicsFluid> fluid = fluidData.fluid.lock();
			if(!fluid)
				continue;

			boost::shared_ptr<physics::Fluid> fluidActor = data->physics->createFluid(physics::PhysicsLib::FluidType(fluidData.type), fluidData.maxParticles, fluidData.fluidStaticRestitution, fluidData.fluidStaticAdhesion, fluidData.fluidDynamicRestitution, fluidData.fluidDynamicAdhesion, fluidData.fluidDamping, fluidData.fluidStiffness, fluidData.fluidViscosity, fluidData.fluidKernelRadiusMultiplier, fluidData.fluidRestParticlesPerMeter, fluidData.fluidRestDensity, fluidData.fluidMotionLimit, fluidData.fluidPacketSizeMultiplier, fluidData.collisionGroup);
			fluid->fluid = fluidActor;
			fluid->lib = data;
		}

		data->fluidList.clear();
	}
}