void  ObjectRenderer::AddShapeFromRay(unsigned uShapeIndex, Vector3 vecRay1, Vector3 vecRay2, double dFactor, double dSphereScale){
    //std::cout << "Adding Sphere on the object's surface. Base point is " << vecRay1.x() << ", " << vecRay1.y() << ", " << vecRay1.z() << ". Vector is " << vecRay2.x() << ", " << vecRay2.y() << ", " << vecRay2.z() << ". Factor is " << dFactor << std::endl;

    // Compute intersection point
    Vector3 vecIntersection = vecRay1 + (vecRay2 - vecRay1)*dFactor;

    //std::cout << " Coordinates: " << vecIntersection.x() << ", " << vecIntersection.y() << ", " << vecIntersection.z() << std::endl;

    // Convert from absolute coordinates to coordinates respective to the object's referential
    Matrix matTmp = mObject->GetReferenceFrame().GetInverse().GetHMatrix();
    double pIntersectionH[4] = { vecIntersection.x(), vecIntersection.y(), vecIntersection.z(), 1.0 };
    Vector vecIntersectionH(pIntersectionH, 4);
    Vector vecTmp = matTmp.Mult(vecIntersectionH);
    vecIntersection.Set(vecTmp);

    //std::cout << "Transformed Coordinates: " << vecIntersection.x() << ", " << vecIntersection.y() << ", " << vecIntersection.z() << std::endl;

    // Create sphere shape
    pShapeStruct pNewShape  = new ShapeStruct;
    pNewShape->strShapeName = mStrIntersectsShapeName;
    pNewShape->color[0]     = 1.0;
    pNewShape->color[1]     = 0.0;
    pNewShape->color[2]     = 0.0;
    pNewShape->color[3]     = 1.0;
    pNewShape->refShape.SetOrigin(vecIntersection);

    double dScale = 0.02;
    double pScaleData[3] = { dScale, dScale, dScale };
    Matrix3 scale;
    scale.Diag(Vector3(pScaleData));

    pNewShape->shape        = new GL3DObject();
    pNewShape->shape->GenerateSphere(16,16);
    pNewShape->shape->Transform(scale);
    pNewShape->shape->Transform(pNewShape->refShape.GetHMatrix());

    mShapes.push_back(pNewShape);


    if(mObject){
        pXmlTree conf = mObject->GetConfigTree();
        pXmlTree ptList = conf->Find("PointList");
        if(ptList==NULL){
            ptList = new XmlTree("PointList");
            conf->AddSubTree(ptList);
        }

        // Apply scale transform if any
        if(mShapes[uShapeIndex]->scale[0]!=1.0 || mShapes[uShapeIndex]->scale[1]!=1.0 || mShapes[uShapeIndex]->scale[2]!=1.0){
            Matrix3 matScale;
            matScale.Diag(Vector3(mShapes[uShapeIndex]->scale[0]));
            matScale.RefNoCheck(0,0) = 1/mShapes[uShapeIndex]->scale[0];
            matScale.RefNoCheck(1,1) = 1/mShapes[uShapeIndex]->scale[1];
            matScale.RefNoCheck(2,2) = 1/mShapes[uShapeIndex]->scale[2];

            Vector3 vecTmp = matScale.Mult(vecIntersection);
            vecIntersection = vecTmp;

            //std::cout << "Scaled Coordinates: " << vecIntersection.x() << ", " << vecIntersection.y() << ", " << vecIntersection.z() << std::endl;
        }

        char txt[256];
        sprintf(txt,"%f %f %f",vecIntersection.x(),vecIntersection.y(),vecIntersection.z());
        ptList->AddSubTree(new XmlTree("Point",txt));
        //conf->Print();
    }
}
ObjectRenderer::pShapeStruct    ObjectRenderer::LoadShape(const pXmlTree tree){
    if(tree==NULL) return NULL;

    REALTYPE *array;

    pXmlTree tmpTree = tree;

    pShapeStruct shape  = NULL;

    if((tmpTree->GetName()=="Shape")){
        gLOG.Append("Setting up Shape : %s",tmpTree->GetData().c_str());
        gLOG.SetDeltaIndent(2);

        int size;
        Matrix3 scale;
        scale.Identity();
        if(tmpTree->Find("Scale")){
            size=tmpTree->GetArray("Scale",&array);
            if(size==3){
                                scale.Diag(Vector3(array));
                                scale.RefNoCheck(0,0) = array[0];
                                scale.RefNoCheck(1,1) = array[1];
                                scale.RefNoCheck(2,2) = array[2];
            }else{
                                gLOG.Append("Error: Bad <Scale> array size (should be 3)");
            }
        }


        string params = tmpTree->Get("Params",string(""));
        vector<string> ptokens = Tokenize(RemoveSpaces(params));

        shape               = new ShapeStruct;
        shape->shape        = new GL3DObject();
        shape->strShapeName = tmpTree->GetData();
        shape->scale[0] = scale.At(0,0);
        shape->scale[1] = scale.At(1,1);
        shape->scale[2] = scale.At(2,2);

              if(tmpTree->GetData().length()==0){
                    gLOG.Append("Error: No shape specified");
        }else if(tmpTree->GetData() == "Cube"){
                shape->shape->GenerateCube();
        }else if(tmpTree->GetData() == "Cylinder"){
                if(ptokens.size()>=1){
                    shape->shape->GenerateCylinder(atoi(ptokens[0].c_str()));
                }else{
                    shape->shape->GenerateCylinder(16);
                }
        }else if(tmpTree->GetData() == "Sphere"){
                if(ptokens.size()>=2){
                    shape->shape->GenerateSphere(atoi(ptokens[0].c_str()),atoi(ptokens[1].c_str()));
                }else{
                    shape->shape->GenerateSphere(16,12);
                }
        }else if(tmpTree->GetData() == "Capsule"){
                if(ptokens.size()==1){
                    shape->shape->GenerateCapsule(atof(ptokens[0].c_str()),16,6);
                }else if(ptokens.size()>=3){
                    shape->shape->GenerateCapsule(atof(ptokens[0].c_str()),atoi(ptokens[1].c_str()),atoi(ptokens[2].c_str()));
                }else{
                    shape->shape->GenerateCapsule(0.5*(scale.RefNoCheck(0,0)+scale.RefNoCheck(1,1)),16,6);
                }
        }else if(tmpTree->GetData() == "HeightField"){
                if(tmpTree->Find("DataFile")){
                    string filename = tmpTree->GetBasePath()+string("/")+tmpTree->Find("DataFile")->GetData();
                    Matrix hf;
                    if(hf.Load(filename.c_str())){
                        shape->shape->GenerateHeightField(hf,1,1,1);
                    }else{
                        delete shape->shape; shape->shape=NULL;
                        gLOG.Append("Error: Height field file %s failed to open",filename.c_str());
                    }
                }else{
                    delete shape->shape; shape->shape=NULL;
                    gLOG.Append("Error: Height field: No <DataFile> specified...");
                }



                /*if(ptokens.size()==1){
                    shape->shape->GenerateCapsule(atof(ptokens[0].c_str()),16,6);
                }else if(ptokens.size()>=3){
                    shape->shape->GenerateCapsule(atof(ptokens[0].c_str()),atoi(ptokens[1].c_str()),atoi(ptokens[2].c_str()));
                }else{
                    shape->shape->GenerateCapsule(0.5*(scale.RefNoCheck(0,0)+scale.RefNoCheck(1,1)),16,6);
                }*/
        }else{
            bool bShapeFound = false;
            string shapeFile;
            if(!bShapeFound){
                shapeFile = tmpTree->GetData();
                bShapeFound = FileFinder::Find(shapeFile);
                if(bShapeFound) shapeFile = FileFinder::GetString();
            }
            if(!bShapeFound){
                shapeFile = mBasePath+"/"+tmpTree->GetData();
                bShapeFound = FileFinder::Find(shapeFile);
                if(bShapeFound) shapeFile = FileFinder::GetString();
            }
            if(!bShapeFound){
                shapeFile = tmpTree->GetBasePath()+"/"+tmpTree->GetData();
                bShapeFound = FileFinder::Find(shapeFile);
                if(bShapeFound) shapeFile = FileFinder::GetString();
            }
            if(bShapeFound){
                if(!shape->shape->LoadFromObjFile(shapeFile.c_str())){
                    delete shape->shape; shape->shape=NULL;
                    gLOG.Append("Error: Unable to load shape file: %s",shapeFile.c_str());
                }
            }else{
                gLOG.Append("Error: Unable to find shape file: %s",shapeFile.c_str());
            }
        }

        if(tmpTree->Find("Origin")){
            size=tmpTree->GetArray("Origin",&array);
            if(size==3){            shape->refShape.SetOrigin().Set(array);
            }else{                  shape->refShape.SetOrigin().Zero();
                                    gLOG.Append("Error: Bad <Origin> array size (should be 3)");
            }
        }else{
                                    shape->refShape.SetOrigin().Zero();
                                    gLOG.Append("Warning: No <Origin> defined");
        }

        if(tmpTree->Find("Orient")){
            size=tmpTree->GetArray("Orient",&array);
            if(size==9){            shape->refShape.SetOrient().Set(array);
                                    shape->refShape.SetOrient().Normalize();
            }else if(size==3){      shape->refShape.SetOrient().SRotationV(Vector3(array));
            }else{                  shape->refShape.SetOrient().Identity();
                                    gLOG.Append("Error: Bad <Orient> array size (should be 3(axis*angle) or 9(full rotation matrix))");
            }
        }else{
                                    shape->refShape.SetOrient().Identity();
                                    gLOG.Append("Warning: No <Orient> defined");
        }
        shape->refShape.Update();

        if(tmpTree->Find("Color")){
            size=tmpTree->GetArray("Color",&array);
            if(size==3){            memcpy(shape->color,array,3*sizeof(REALTYPE));
                                    shape->color[3] = 1.0;
            }else if(size==4){      memcpy(shape->color,array,4*sizeof(REALTYPE));
            }else{                  shape->color[0] = 1.0;
                                    shape->color[1] = 1.0;
                                    shape->color[2] = 1.0;
                                    shape->color[3] = 1.0;
                                    gLOG.Append("Error: Bad <Color> array size (should be 3 or 4)");
            }
        }else{
                                    shape->color[0] = 1.0;
                                    shape->color[1] = 1.0;
                                    shape->color[2] = 1.0;
                                    shape->color[3] = 1.0;
                                    gLOG.Append("Warning: No <Color> defined");
        }
        if(tmpTree->Find("DoubleFace")){
        	shape->culling = tmpTree->Get("DoubleFace",false);
        }
        else
        {
        	shape->culling = false;
        }

        if(shape->shape!=NULL){
            shape->shape->Transform(scale);
            shape->shape->Transform(shape->refShape.GetHMatrix());

            /*
            pXmlTree tmpShadowTree;
            if((tmpShadowTree=tmpTree->Find("Shadow"))!=NULL){
                string params = tmpShadowTree->Get("Params",string(""));
                vector<string> ptokens = Tokenize(RemoveSpaces(params));

                GL3DObject *shadow = new GL3DObject();
                      if(tmpShadowTree->GetData() == "Cube"){
                        shadow->GenerateCube();
                }else if(tmpShadowTree->GetData() == "Cylinder"){
                        if(ptokens.size()>=1){
                            shadow->GenerateCylinder(atoi(ptokens[0].c_str()));
                        }else{
                            shadow->GenerateCylinder(8);
                        }
                }else if(tmpShadowTree->GetData() == "Sphere"){
                        if(ptokens.size()>=2){
                            shadow->GenerateSphere(atoi(ptokens[0].c_str()),atoi(ptokens[1].c_str()));
                        }else{
                            shadow->GenerateSphere(4,4);
                        }
                }else if(tmpShadowTree->GetData() == "Clone"){
                        delete shadow;
                        shadow=shape->shape->Clone();
                }else{
                    if(!shadow->LoadFromObjFile(tmpShadowTree->GetData().c_str(),true)){
                        delete shadow; shadow=NULL;
                    }
                }
                if(shadow!=NULL){
                    shadow->Transform(shape->refShape.GetHMatrix());

                    if(tmpShadowTree->Find("Origin")){
                        Vector3 origin;
                        origin.Set(Vector(array,tmpShadowTree->GetArray("Origin",&array)));
                        shadow->AddOffset(origin);
                    }
                    if(tmpShadowTree->Find("Orient")){
                        Matrix3 orient;
                        orient.Set(Matrix(array,tmpShadowTree->GetArray("Orient",&array)/3,3));
                        shadow->Transform(orient);
                    }
                    shape->shape->SetShadow(shadow);
                }
            }
            */
        }
        gLOG.SetDeltaIndent(-2);

    }


    return shape;
}