GameInstanceServer::ChunkCoordinates GameInstanceServer::positionToChunk(const osg::Vec3f& pos)
{
    return ChunkCoordinates(
               std::floor(pos.x() / chunksize),
               std::floor(pos.y() / chunksize),
               std::floor(pos.z() / chunksize));
}

unsigned int PhysicsEngine::addCollisionSphere(osg::Vec3f pos, float radius, float mass)
{
    assert(OpenThreads::Thread::CurrentThread() == m_physicsThread);

    btSphereShape* newShape = new btSphereShape(radius);
    m_collisionShapes.push_back(newShape);

    btTransform initialTransform;
    initialTransform.setIdentity();
    initialTransform.setOrigin(btVector3(pos.x(), pos.y(), pos.z()));

    //btMotionState* motionState = new btDefaultMotionState(initialTransform);
    //btVector3 inertia;
    //newShape->calculateLocalInertia(0.0, inertia);

    // TODO: upgrade to the rigid body status?
    btCollisionObject* newBody = new btCollisionObject;
    newBody->setCollisionShape(newShape);
    newBody->setWorldTransform(initialTransform);
    m_collisionObjects.insert(std::make_pair(m_nextUsedId, newBody));
    ++m_nextUsedId;

    m_physicsWorld->addCollisionObject(newBody, COLLISION_ASTEROIDGROUP, COLLISION_SHIPGROUP);
    return m_nextUsedId - 1;
}

std::string toString(const osg::Vec3f &value)
{
    std::stringstream str;

    str << value.x() << " " << value.y() << " " << value.z();
    return str.str();
}

 void Storage::fixNormal (osg::Vec3f& normal, int cellX, int cellY, int col, int row)
 {
     while (col >= ESM::Land::LAND_SIZE-1)
     {
         ++cellY;
         col -= ESM::Land::LAND_SIZE-1;
     }
     while (row >= ESM::Land::LAND_SIZE-1)
     {
         ++cellX;
         row -= ESM::Land::LAND_SIZE-1;
     }
     while (col < 0)
     {
         --cellY;
         col += ESM::Land::LAND_SIZE-1;
     }
     while (row < 0)
     {
         --cellX;
         row += ESM::Land::LAND_SIZE-1;
     }
     ESM::Land* land = getLand(cellX, cellY);
     if (land && land->mDataTypes&ESM::Land::DATA_VNML)
     {
         normal.x() = land->mLandData->mNormals[col*ESM::Land::LAND_SIZE*3+row*3];
         normal.y() = land->mLandData->mNormals[col*ESM::Land::LAND_SIZE*3+row*3+1];
         normal.z() = land->mLandData->mNormals[col*ESM::Land::LAND_SIZE*3+row*3+2];
         normal.normalize();
     }
     else
         normal = osg::Vec3f(0,0,1);
 }

 osg::Vec3f CoordinateConverter::toLocalVec3(const osg::Vec3f& point)
 {
     return osg::Vec3f(
         point.x() - static_cast<float>(mCellX),
         point.y() - static_cast<float>(mCellY),
         point.z()
     );
 }

GeometryTransitPtr CSGGeometry::toOsgGeometry(CGAL::Polyhedron *p) {
	GeoPnt3fPropertyRecPtr positions = GeoPnt3fProperty::create();
	GeoVec3fPropertyRecPtr normals = GeoVec3fProperty::create();
	GeoUInt32PropertyRecPtr indices = GeoUInt32Property::create();

	/*
	 * Iterate over all faces, add their vertices to 'positions' && write indices at
	 * the same time. Results in no shared vertices && therefore no normal interpolation between
	 * faces, but makes cubes look good. Well, well...
	 */

	Matrix localToWorld = getWorldMatrix();
	OSG::Vec3f translation;
	OSG::Quaternion rotation;
	OSG::Vec3f scaleFactor;
	OSG::Quaternion scaleOrientation;
	localToWorld.getTransform(translation, rotation, scaleFactor, scaleOrientation);
	Matrix worldToLocal;
	worldToLocal.invertFrom(localToWorld);

	// Convert indices && positions
	int curIndex = 0;
	for (CGAL::Polyhedron::Facet_const_iterator it = p->facets_begin(); it != p->facets_end(); it++) {
		CGAL::Polyhedron::Halfedge_around_facet_const_circulator circ = it->facet_begin();
		do {
			CGAL::Point cgalPos = circ->vertex()->point();
			// We need to transform each point from global coordinates into our local coordinate system
			// (CGAL uses global, OpenSG has geometry in node-local coords)
			OSG::Vec3f vecPos = OSG::Vec3f(CGAL::to_double(cgalPos.x()),
											  CGAL::to_double(cgalPos.y()),
											  CGAL::to_double(cgalPos.z()));
			OSG::Vec3f localVec = worldToLocal * (vecPos - translation);
			OSG::Pnt3f osgPos(localVec.x(), localVec.y(), localVec.z());

			positions->addValue(osgPos);
			normals->addValue(Vec3f(0,1,0));
			indices->addValue(curIndex);
			curIndex++;
		} while (++circ != it->facet_begin());
	}

	GeoUInt8PropertyRecPtr types = GeoUInt8Property::create();
	types->addValue(GL_TRIANGLES);
	GeoUInt32PropertyRecPtr lengths = GeoUInt32Property::create();
	lengths->addValue(indices->size());

	GeometryRecPtr mesh = Geometry::create();
	mesh->setPositions(positions);
	mesh->setNormals(normals);
	mesh->setIndices(indices);
	mesh->setTypes(types);
	mesh->setLengths(lengths);
	mesh->setMaterial(VRMaterial::getDefault()->getMaterial());
    createSharedIndex(mesh);
	calcVertexNormals(mesh, 0.523598775598 /*30 deg in rad*/);

	return GeometryTransitPtr(mesh);
}

void RippleSimulation::emitRipple(const osg::Vec3f &pos)
{
    if (std::abs(pos.z() - mParticleNode->getPosition().z()) < 20)
    {
        osgParticle::Particle* p = mParticleSystem->createParticle(NULL);
        p->setPosition(osg::Vec3f(pos.x(), pos.y(), 0.f));
        p->setAngle(osg::Vec3f(0,0, Misc::Rng::rollProbability() * osg::PI * 2 - osg::PI));
    }
}

//***********************************************************
//FUNCTION:
void CPickNode::__drawCoordinateLine(const osg::Vec3f& vOrigin, float vLength, osg::ref_ptr<osg::Geode>& vLineNode)
{
	osg::ref_ptr<osg::Geometry> CoordGeometry = new osg::Geometry();
	osg::ref_ptr<osg::Vec3Array> CoordVertex = new osg::Vec3Array();

	CoordVertex->push_back(vOrigin);
	CoordVertex->push_back(osg::Vec3(vOrigin.x()+vLength, vOrigin.y(), vOrigin.z()));
	CoordVertex->push_back(vOrigin);
	CoordVertex->push_back(osg::Vec3(vOrigin.x(), vOrigin.y()+vLength, vOrigin.z()));
	CoordVertex->push_back(vOrigin);
	CoordVertex->push_back(osg::Vec3(vOrigin.x(), vOrigin.y(), vOrigin.z()+vLength));
	CoordGeometry->setVertexArray(CoordVertex.get());
	CoordGeometry->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 6));

	osg::ref_ptr<osg::Vec4Array> VertColor = new osg::Vec4Array();
	VertColor->push_back(osg::Vec4(1.0, 0.0, 0.0, 1.0));
	VertColor->push_back(osg::Vec4(1.0, 0.0, 0.0, 1.0));
	VertColor->push_back(osg::Vec4(0.0, 1.0, 0.0, 1.0));
	VertColor->push_back(osg::Vec4(0.0, 1.0, 0.0, 1.0));
	VertColor->push_back(osg::Vec4(0.0, 0.0, 1.0, 1.0));
	VertColor->push_back(osg::Vec4(0.0, 0.0, 1.0, 1.0));
	CoordGeometry->setColorArray(VertColor.get());
	CoordGeometry->setColorBinding(osg::Geometry::BIND_PER_VERTEX);

	osg::ref_ptr<osg::LineWidth> LineSize = new osg::LineWidth();
	LineSize->setWidth(5);
	vLineNode->getOrCreateStateSet()->setAttributeAndModes(LineSize.get(), osg::StateAttribute::ON);
	vLineNode->addDrawable(CoordGeometry.get());
}

OSG_BASE_DLLMAPPING bool MatrixLookAt(OSG::Matrix &result,
                                      OSG::Pnt3f   from, 
                                      OSG::Pnt3f   at, 
                                      OSG::Vec3f   up    )
{
    Vec3f view;
    Vec3f right;
    Vec3f newup;
    Vec3f tmp;

    view = from - at;
    view.normalize();

    right = up.cross(view);

    if(right.dot(right) < TypeTraits<Real>::getDefaultEps())
    {
        return true;
    }

    right.normalize();

    newup = view.cross(right);

    result.setIdentity ();
    result.setTranslate(from[0], from[1], from[2]);

    Matrix tmpm;

    tmpm.setValue(right, newup, view);

    result.mult(tmpm);

    return false;
}

// Convenience function to calculate the translation base on a time and
// direction parameter.
OSG::Vec3f calcTranslation(OSG::Real32 t, OSG::Vec3f dir)
{
   // The 'dir' vector encodes direction and velocity
   OSG::Real32 v = dir.length();
   OSG::Real32 s = v * t;
   return(dir * s);
}

osg::Vec3f ShapeVisitor_RestrictedPositionGetter::toCube( const osg::Vec3f& center, const osg::Vec3f& surfaceX, const osg::Vec3f& surfaceY, const osg::Vec3f& surfaceZ, const osg::Vec3f& point )
{
	float distanceX = std::fabs( ( center - surfaceX ).x() );
	float distanceY = std::fabs( ( center - surfaceY ).y() );
	float distanceZ = std::fabs( ( center - surfaceZ ).z() );

	//  nearest_point_on_box(x, y, z, box_min_x, box_min_y, box_min_z, box_max_x, box_max_y, box_max_z)
	float x = /*point.x() -*/ median( point.x(), center.x() - distanceX, center.x() + distanceX );
	float y = /*point.y() -*/ median( point.y(), center.y() - distanceY, center.y() + distanceY );
	float z = /*point.z() -*/ median( point.z(), center.z() - distanceZ, center.z() + distanceZ );

	return osg::Vec3f( x,y,z );
}

bool
TritonContext::intersect(const osg::Vec3d& start, const osg::Vec3d& dir, float& out_height, osg::Vec3f& out_normal) const
{
    ::Triton::Vector3 p(start.ptr());
    ::Triton::Vector3 d(dir.ptr());
    ::Triton::Vector3 normal;
    bool ok = _ocean->GetHeight(p, d, out_height, normal);
    out_normal.set(normal.x, normal.y, normal.z);
    return ok;
}

 void Storage::averageNormal(osg::Vec3f &normal, int cellX, int cellY, int col, int row)
 {
     osg::Vec3f n1,n2,n3,n4;
     fixNormal(n1, cellX, cellY, col+1, row);
     fixNormal(n2, cellX, cellY, col-1, row);
     fixNormal(n3, cellX, cellY, col, row+1);
     fixNormal(n4, cellX, cellY, col, row-1);
     normal = (n1+n2+n3+n4);
     normal.normalize();
 }

    void Scene::playerMoved(const osg::Vec3f &pos)
    {
        if (!mCurrentCell || !mCurrentCell->isExterior())
            return;

        // figure out the center of the current cell grid (*not* necessarily mCurrentCell, which is the cell the player is in)
        int cellX, cellY;
        getGridCenter(cellX, cellY);
        float centerX, centerY;
        MWBase::Environment::get().getWorld()->indexToPosition(cellX, cellY, centerX, centerY, true);
        const float maxDistance = 8192/2 + 1024; // 1/2 cell size + threshold
        float distance = std::max(std::abs(centerX-pos.x()), std::abs(centerY-pos.y()));
        if (distance > maxDistance)
        {
            int newX, newY;
            MWBase::Environment::get().getWorld()->positionToIndex(pos.x(), pos.y(), newX, newY);
            changeCellGrid(newX, newY);
            //mRendering.updateTerrain();
        }
    }

//***********************************************************
//FUNCTION:
void CPickNode::__drawCoordinateMask(const osg::Vec3f& vOrigin, float vLength, osg::ref_ptr<osg::Geode>& vMaskNode)
{
	osg::ref_ptr<osg::Geometry> MakGeom = new osg::Geometry();
	osg::ref_ptr<osg::Vec3Array> MaskVert = new osg::Vec3Array();

	for (unsigned int i=1; i<=10; i++)
	{
		float Offfset = vLength / 10.0 * i;
		MaskVert->push_back(osg::Vec3(vOrigin.x()+Offfset, vOrigin.y(), vOrigin.z()));
		MaskVert->push_back(osg::Vec3(vOrigin.x(), vOrigin.y()+Offfset, vOrigin.z()));
		MaskVert->push_back(osg::Vec3(vOrigin.x(), vOrigin.y(), vOrigin.z()+Offfset));
	}
	osg::ref_ptr<osg::Vec4Array> ColorArray = new osg::Vec4Array;
	ColorArray->push_back(osg::Vec4(1.0, 1.0, 1.0, 1.0));

	MakGeom->setVertexArray(MaskVert.get());
	MakGeom->setColorArray(ColorArray);
	MakGeom->setColorBinding(osg::Geometry::BIND_OVERALL);

	MakGeom->addPrimitiveSet(new osg::DrawArrays(osg::PrimitiveSet::POINTS, 0, 30));
	vMaskNode->addDrawable(MakGeom.get());

	osg::ref_ptr<osg::Point> PointSize = new osg::Point(5.0);
	osg::ref_ptr<osg::StateSet> PointStateSet = vMaskNode->getOrCreateStateSet();
	PointStateSet->setAttribute(PointSize);
}

osg::Vec4f Raytracer::renderPixel(int x, int y, const osg::Vec3f& eye, const osg::Vec3f& dir)
{
    osg::Vec3f lightPos(-338.572, 0, 400);
    RTCRay ray;
    ray.primID = RTC_INVALID_GEOMETRY_ID;
    ray.geomID = RTC_INVALID_GEOMETRY_ID;
    ray.instID = RTC_INVALID_GEOMETRY_ID;
    ray.tnear = 0.0f;
    ray.tfar = 100000.0f;
    ray.org[0] = eye.x();
    ray.org[1] = eye.y();
    ray.org[2] = eye.z();
    ray.dir[0]=  dir.x();
    ray.dir[1] = dir.y();
    ray.dir[2] = dir.z();
    rtcIntersect(_scene, ray);
    if(ray.primID != -1)
    {
        osg::Vec3f pos = eye + dir*ray.tfar;
        osg::Vec4f diffuse = getSurfaceColor(ray.geomID, ray.primID,ray.u,ray.v);
        osg::Vec3 normal = getNormal(ray.geomID,ray.primID,ray.u,ray.v);
        osg::Vec3f lightDir = lightPos - pos;
        lightDir.normalize();

        float NdotL = std::max(normal * lightDir,0.0f);
        return diffuse * NdotL;

    }
    return _backgroundColor;
}

void ActorTracer::findGround(const Actor* actor, const osg::Vec3f& start, const osg::Vec3f& end, const btCollisionWorld* world)
{
    const btVector3 btstart(start.x(), start.y(), start.z());
    const btVector3 btend(end.x(), end.y(), end.z());

    const btTransform &trans = actor->getCollisionObject()->getWorldTransform();
    btTransform from(trans.getBasis(), btstart);
    btTransform to(trans.getBasis(), btend);

    ClosestNotMeConvexResultCallback newTraceCallback(actor->getCollisionObject(), btstart-btend, btScalar(0.0));
    // Inherit the actor's collision group and mask
    newTraceCallback.m_collisionFilterGroup = actor->getCollisionObject()->getBroadphaseHandle()->m_collisionFilterGroup;
    newTraceCallback.m_collisionFilterMask = actor->getCollisionObject()->getBroadphaseHandle()->m_collisionFilterMask;
    newTraceCallback.m_collisionFilterMask &= ~CollisionType_Actor;

    world->convexSweepTest(actor->getConvexShape(), from, to, newTraceCallback);
    if(newTraceCallback.hasHit())
    {
        const btVector3& tracehitnormal = newTraceCallback.m_hitNormalWorld;
        mFraction = newTraceCallback.m_closestHitFraction;
        mPlaneNormal = osg::Vec3f(tracehitnormal.x(), tracehitnormal.y(), tracehitnormal.z());
        mEndPos = (end-start)*mFraction + start;
    }
    else
    {
        mEndPos = end;
        mPlaneNormal = osg::Vec3f(0.0f, 0.0f, 1.0f);
        mFraction = 1.0f;
    }
}

unsigned int PhysicsEngine::addUserVehicle(const osg::Vec3f& pos, const osg::Vec3f& sizes, const osg::Quat& orient, float mass)
{
    assert(OpenThreads::Thread::CurrentThread() == m_physicsThread);

    if (! m_vehicleShape)
        m_vehicleShape = new btBoxShape(btVector3(sizes.x() / 2, sizes.y() / 2, sizes.z() / 2));
    //m_collisionShapes.push_back(m_vehicleShape);

    btTransform startTransform;
    startTransform.setIdentity();
    startTransform.setRotation(btQuaternion(orient.x(), orient.y(), orient.z(), orient.w()));
    startTransform.setOrigin(btVector3(pos.x(), pos.y(), pos.z()));

    VehicleMotionState* motionState = new VehicleMotionState(this, m_nextUsedId, startTransform);
    btVector3 inertia;
    m_vehicleShape->calculateLocalInertia(mass, inertia);

    btRigidBody* body = new btRigidBody(mass, motionState, m_vehicleShape, inertia);
    m_vehicles[m_nextUsedId] = body;
    ++m_nextUsedId;

    body->setDamping(0.5f, 0.7f);
    m_physicsWorld->addRigidBody(body, COLLISION_SHIPGROUP, COLLISION_SHIPGROUP | COLLISION_ASTEROIDGROUP);

    return m_nextUsedId - 1;
}

/*
 Linearly interpolate the position where an isosurface cuts
 an edge between two vertices, each with their own scalar value
 */
osg::Vec3f VertexInterp(double isolevel, osg::Vec3f p1, osg::Vec3f p2, double valp1, double valp2)
{
	double mu;
	osg::Vec3f p;
	
	if (ABS(isolevel-valp1) < 0.00001)
		return p1;
	if (ABS(isolevel-valp2) < 0.00001)
		return p2;
	if (ABS(valp1-valp2) < 0.00001)
		return p1;
	
	mu = (isolevel - valp1) / (valp2 - valp1);
	
	p.set(p1.x() + mu * (p2.x() - p1.x()),
		  p1.y() + mu * (p2.y() - p1.y()),
		  p1.z() + mu * (p2.z() - p1.z()));
	
	return p;
}

const osg::Vec3f Stars::sRgbColor(const osg::Vec3f xyzTristimulus)
{
    // (The Colors of the Stars - Olson - 1998) - sRGB matrix - feel free to use other matrices here...

    const float r =  3.24  * xyzTristimulus.x() -1.537 * xyzTristimulus.y() -0.499 * xyzTristimulus.z();
    const float g = -0.969 * xyzTristimulus.x() +1.876 * xyzTristimulus.y() +0.042 * xyzTristimulus.z();
    const float b =  0.056 * xyzTristimulus.x() -0.204 * xyzTristimulus.y() +1.057 * xyzTristimulus.z();

    return osg::Vec3f(r, g, b);
}