diff --git a/release/scripts/ui/properties_data_modifier.py b/release/scripts/ui/properties_data_modifier.py
index b7b4c8dc64c..29034997754 100644
--- a/release/scripts/ui/properties_data_modifier.py
+++ b/release/scripts/ui/properties_data_modifier.py
@@ -462,7 +462,12 @@ class DATA_PT_modifiers(DataButtonsPanel):
             row.label()
 
     def NAVMESH(self, layout, ob, md, wide_ui):
-        layout = self.layout
+        split = layout.split()
+        if ob.mode == 'EDIT':
+            col = split.column()
+            col.operator("object.assign_navpolygon", text="Assign poly idx")
+            col = split.column()
+            col.operator("object.assign_new_navpolygon", text="Assign new poly idx")
 
     def PARTICLE_INSTANCE(self, layout, ob, md, wide_ui):
         layout.prop(md, "object")
diff --git a/source/blender/editors/object/object_intern.h b/source/blender/editors/object/object_intern.h
index e30a6c555e3..7b2904eed9d 100644
--- a/source/blender/editors/object/object_intern.h
+++ b/source/blender/editors/object/object_intern.h
@@ -224,6 +224,8 @@ void OBJECT_OT_bake_image(wmOperatorType *ot);
 
 /* object_navmesh.cpp */
 void OBJECT_OT_create_navmesh(struct wmOperatorType *ot);
+void OBJECT_OT_assign_navpolygon(struct wmOperatorType *ot);
+void OBJECT_OT_assign_new_navpolygon(struct wmOperatorType *ot);
 
 #endif /* ED_OBJECT_INTERN_H */
 
diff --git a/source/blender/editors/object/object_navmesh.cpp b/source/blender/editors/object/object_navmesh.cpp
index a5ff74ae4fd..f7527e2d5d5 100644
--- a/source/blender/editors/object/object_navmesh.cpp
+++ b/source/blender/editors/object/object_navmesh.cpp
@@ -50,6 +50,8 @@ extern "C"
 #include "ED_object.h"
 #include "BLI_math_vector.h"
 
+#include "RNA_access.h"
+
 #include "ED_mesh.h"
 
 /*mesh/mesh_intern.h */
@@ -372,7 +374,7 @@ static Object* createRepresentation(bContext *C, rcPolyMesh*& pmesh, rcPolyMeshD
 
 			//set navigation polygon idx to the custom layer
 			int* polygonIdx = (int*)CustomData_em_get(&em->fdata, newFace->data, CD_PROP_INT);
-			*polygonIdx = i;
+			*polygonIdx = i+1; //add 1 to avoid zero idx
 		}
 		
 		EM_free_index_arrays();
@@ -425,7 +427,7 @@ static int create_navmesh_exec(bContext *C, wmOperator *op)
 void OBJECT_OT_create_navmesh(wmOperatorType *ot)
 {
 	/* identifiers */
-	ot->name= "NavMesh";
+	ot->name= "Create navigation mesh";
 	ot->description= "Create navigation mesh for selected objects";
 	ot->idname= "OBJECT_OT_create_navmesh";
 
@@ -435,4 +437,145 @@ void OBJECT_OT_create_navmesh(wmOperatorType *ot)
 	/* flags */
 	ot->flag= OPTYPE_REGISTER|OPTYPE_UNDO;
 }
+
+static int assign_navpolygon_poll(bContext *C)
+{
+	Object *ob= (Object *)CTX_data_pointer_get_type(C, "object", &RNA_Object).data;
+	if (!ob || !ob->data)
+		return 0;
+	return (((Mesh*)ob->data)->edit_mesh != NULL);
+}
+
+static int assign_navpolygon_exec(bContext *C, wmOperator *op)
+{
+	Object *obedit= CTX_data_edit_object(C);
+	EditMesh *em= BKE_mesh_get_editmesh((Mesh *)obedit->data);
+
+	//do work here
+	int targetPolyIdx = -1;
+	EditFace *ef, *efa;
+	efa = EM_get_actFace(em, 0);
+	if (efa) 
+	{
+		if (CustomData_has_layer(&em->fdata, CD_PROP_INT))
+		{
+			targetPolyIdx = *(int*)CustomData_em_get(&em->fdata, efa->data, CD_PROP_INT);
+			targetPolyIdx = targetPolyIdx>=0? targetPolyIdx : -targetPolyIdx;
+			if (targetPolyIdx>0)
+			{
+				//set target poly idx to other selected faces
+				ef = (EditFace*)em->faces.last;
+				while(ef) 
+				{
+					if((ef->f & SELECT )&& ef!=efa) 
+					{
+						int* recastDataBlock = (int*)CustomData_em_get(&em->fdata, ef->data, CD_PROP_INT);
+						*recastDataBlock = targetPolyIdx;
+					}
+					ef = ef->prev;
+				}
+			}
+		}		
+	}
+	
+	DAG_id_flush_update((ID*)obedit->data, OB_RECALC_DATA);
+	WM_event_add_notifier(C, NC_GEOM|ND_DATA, obedit->data);
+
+	BKE_mesh_end_editmesh((Mesh*)obedit->data, em);
+	return OPERATOR_FINISHED;
+}
+
+void OBJECT_OT_assign_navpolygon(struct wmOperatorType *ot)
+{
+	/* identifiers */
+	ot->name= "Assign polygon index ";
+	ot->description= "Assign polygon index to face by active face";
+	ot->idname= "OBJECT_OT_assign_navpolygon";
+
+	/* api callbacks */
+	ot->poll = assign_navpolygon_poll;
+	ot->exec= assign_navpolygon_exec;
+
+	/* flags */
+	ot->flag= OPTYPE_REGISTER|OPTYPE_UNDO;
+}
+
+static int compare(const void * a, const void * b){  
+	return ( *(int*)a - *(int*)b );
+}
+static int findFreeNavPolyIndex(EditMesh* em)
+{
+	//construct vector of indices
+	int numfaces = em->totface;
+	int* indices = new int[numfaces];
+	EditFace* ef = (EditFace*)em->faces.last;
+	int idx = 0;
+	while(ef) 
+	{
+		int polyIdx = *(int*)CustomData_em_get(&em->fdata, ef->data, CD_PROP_INT);
+		indices[idx] = polyIdx;
+		idx++;
+		ef = ef->prev;
+	}
+	qsort(indices, numfaces, sizeof(int), compare);
+	//search first free index
+	int freeIdx = 1;
+	int maxIdx = indices[numfaces-1];
+	for (int i=0; i<numfaces; i++)
+	{
+		if (indices[i]==freeIdx)
+			freeIdx++;
+		else if (indices[i]>freeIdx)
+			break;
+	}
+	delete indices;
+	return freeIdx;
+}
+
+static int assign_new_navpolygon_exec(bContext *C, wmOperator *op)
+{
+	Object *obedit= CTX_data_edit_object(C);
+	EditMesh *em= BKE_mesh_get_editmesh((Mesh *)obedit->data);
+
+	EditFace *ef;
+	if (CustomData_has_layer(&em->fdata, CD_PROP_INT))
+	{
+		int targetPolyIdx = findFreeNavPolyIndex(em);
+		if (targetPolyIdx>0)
+		{
+			//set target poly idx to selected faces
+			ef = (EditFace*)em->faces.last;
+			while(ef) 
+			{
+				if(ef->f & SELECT ) 
+				{
+					int* recastDataBlock = (int*)CustomData_em_get(&em->fdata, ef->data, CD_PROP_INT);
+					*recastDataBlock = targetPolyIdx;
+				}
+				ef = ef->prev;
+			}
+		}
+	}		
+
+	DAG_id_flush_update((ID*)obedit->data, OB_RECALC_DATA);
+	WM_event_add_notifier(C, NC_GEOM|ND_DATA, obedit->data);
+
+	BKE_mesh_end_editmesh((Mesh*)obedit->data, em);
+	return OPERATOR_FINISHED;
+}
+
+void OBJECT_OT_assign_new_navpolygon(struct wmOperatorType *ot)
+{
+	/* identifiers */
+	ot->name= "Assign new polygon index ";
+	ot->description= "Assign new polygon index to face";
+	ot->idname= "OBJECT_OT_assign_new_navpolygon";
+
+	/* api callbacks */
+	ot->poll = assign_navpolygon_poll;
+	ot->exec= assign_new_navpolygon_exec;
+
+	/* flags */
+	ot->flag= OPTYPE_REGISTER|OPTYPE_UNDO;
+}
 }
diff --git a/source/blender/editors/object/object_ops.c b/source/blender/editors/object/object_ops.c
index 0a031b8af9c..784818658a6 100644
--- a/source/blender/editors/object/object_ops.c
+++ b/source/blender/editors/object/object_ops.c
@@ -211,6 +211,8 @@ void ED_operatortypes_object(void)
 	WM_operatortype_append(OBJECT_OT_drop_named_material);
 
 	WM_operatortype_append(OBJECT_OT_create_navmesh);
+	WM_operatortype_append(OBJECT_OT_assign_navpolygon);
+	WM_operatortype_append(OBJECT_OT_assign_new_navpolygon);
 }
 
 void ED_operatormacros_object(void)
diff --git a/source/blender/modifiers/intern/MOD_navmesh.cpp b/source/blender/modifiers/intern/MOD_navmesh.cpp
index 67f06bcf958..6bf2400b3f6 100644
--- a/source/blender/modifiers/intern/MOD_navmesh.cpp
+++ b/source/blender/modifiers/intern/MOD_navmesh.cpp
@@ -42,20 +42,11 @@ extern "C"{
 #include "GPU_draw.h"
 #include "UI_resources.h"
 
-
-static void initData(ModifierData *md)
+//service function
+inline int abs(int a)
 {
-	NavMeshModifierData *nmmd = (NavMeshModifierData*) md;
+	return a>=0 ? a: -a;
 }
-
-static void copyData(ModifierData *md, ModifierData *target)
-{
-	NavMeshModifierData *nmmd = (NavMeshModifierData*) md;
-	NavMeshModifierData *tnmmd = (NavMeshModifierData*) target;
-
-	//.todo - deep copy
-}
-
 inline int bit(int a, int b)
 {
 	return (a & (1 << b)) >> b;
@@ -70,6 +61,387 @@ inline void intToCol(int i, float* col)
 	col[1] = 1 - g*63.0f/255.0f;
 	col[2] = 1 - b*63.0f/255.0f;
 }
+
+inline float area2(const float* a, const float* b, const float* c)
+{
+	return (b[0] - a[0]) * (c[2] - a[2]) - (c[0] - a[0]) * (b[2] - a[2]);
+}
+inline bool left(const float* a, const float* b, const float* c)
+{
+	return area2(a, b, c) < 0;
+}
+
+inline int polyNumVerts(const unsigned short* p, const int vertsPerPoly)
+{
+	int nv = 0;
+	for (int i=0; i<vertsPerPoly; i++)
+	{
+		if (p[i]==0xffff)
+			break;
+		nv++;
+	}
+	return nv;
+}
+
+inline bool polyIsConvex(const unsigned short* p, const int vertsPerPoly, const float* verts)
+{
+	int nv = polyNumVerts(p, vertsPerPoly);
+	if (nv<3)
+		return false;
+	for (int j=0; j<nv; j++)
+	{
+		const float* v = &verts[3*p[j]];
+		const float* v_next = &verts[3*p[(j+1)%nv]];
+		const float* v_prev = &verts[3*p[(nv+j-1)%nv]];
+		if (!left(v_prev, v, v_next))
+			return false;
+
+	}
+	return true;
+}
+
+static float distPointToSegmentSq(const float* point, const float* a, const float* b)
+{
+	float abx[3], dx[3];
+	vsub(abx, b,a);
+	vsub(dx, point,a);
+	float d = abx[0]*abx[0]+abx[2]*abx[2];
+	float t = abx[0]*dx[0]+abx[2]*dx[2];
+	if (d > 0)
+		t /= d;
+	if (t < 0)
+		t = 0;
+	else if (t > 1)
+		t = 1;
+	dx[0] = a[0] + t*abx[0] - point[0];
+	dx[2] = a[2] + t*abx[2] - point[2];
+	return dx[0]*dx[0] + dx[2]*dx[2];
+}
+
+static bool buildRawVertIndicesData(DerivedMesh* dm, int &nverts, float *&verts, 
+									int &ntris, unsigned short *&tris, int *&trisToFacesMap,
+									int *&recastData)
+{
+	nverts = dm->getNumVerts(dm);
+	verts = new float[3*nverts];
+	dm->getVertCos(dm, (float(*)[3])verts);
+	
+	//flip coordinates
+	for (int vi=0; vi<nverts; vi++)
+	{
+		SWAP(float, verts[3*vi+1], verts[3*vi+2]);
+	}
+
+	//calculate number of tris
+	int nfaces = dm->getNumFaces(dm);
+	MFace *faces = dm->getFaceArray(dm);
+	ntris = nfaces;
+	for (int fi=0; fi<nfaces; fi++)
+	{
+		MFace* face = &faces[fi];
+		if (face->v4)
+			ntris++;
+	}
+
+	//copy and transform to triangles (reorder on the run)
+	trisToFacesMap = new int[ntris];
+	tris = new unsigned short[3*ntris];
+	unsigned short* tri = tris;
+	int triIdx = 0;
+	for (int fi=0; fi<nfaces; fi++)
+	{
+		MFace* face = &faces[fi];
+		tri[3*triIdx+0] = face->v1;
+		tri[3*triIdx+1] = face->v3;
+		tri[3*triIdx+2] = face->v2;
+		trisToFacesMap[triIdx++]=fi;
+		if (face->v4)
+		{
+			tri[3*triIdx+0] = face->v1;
+			tri[3*triIdx+1] = face->v4;
+			tri[3*triIdx+2] = face->v2;
+			trisToFacesMap[triIdx++]=fi;
+		}
+	}
+
+	//carefully, recast data is just reference to data in derived mesh
+	recastData = (int*)CustomData_get_layer(&dm->faceData, CD_PROP_INT);
+	return true;
+}
+
+static bool buildPolygonsByDetailedMeshes(const int vertsPerPoly, const int npolys, 
+										  unsigned short* polys, const unsigned short* dmeshes, 
+										  const float* verts, const unsigned short* dtris, 
+										  const int* dtrisToPolysMap)
+{
+	bool res = false;
+	int capacity = vertsPerPoly;
+	unsigned short* newPoly =  new unsigned short[capacity];
+	memset(newPoly, 0xff, sizeof(unsigned short)*capacity);
+	for (int polyidx=0; polyidx<npolys; polyidx++)
+	{
+		int nv = 0;
+		//search border 
+		int btri = -1;
+		int bedge = -1;
+		for (int j=0; j<dmeshes[polyidx*4+3] && btri==-1;j++)
+		{
+			int curpolytri = dmeshes[polyidx*4+2]+j;
+			for (int k=0; k<3; k++)
+			{
+				unsigned short neighbortri = dtris[curpolytri*3*2+3+k];
+				if ( neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
+				{
+					btri = curpolytri;
+					bedge = k;
+					break;
+				}
+			}							
+		}
+		if (btri==-1 || bedge==-1)
+		{
+			//can't find triangle with border edge
+			return false;
+		}
+
+		newPoly[nv++] = dtris[btri*3*2+bedge];
+
+		int tri = btri;
+		int edge = (bedge+1)%3;
+		while (tri!=btri || edge!=bedge)
+		{
+			int neighbortri = dtris[tri*3*2+3+edge];
+			if (neighbortri==0xffff || dtrisToPolysMap[neighbortri]!=polyidx+1)
+			{
+				if (nv==capacity)
+				{
+					capacity += vertsPerPoly;
+					unsigned short* newPolyBig =  new unsigned short[capacity];
+					memset(newPolyBig, 0xff, sizeof(unsigned short)*capacity);
+					memcpy(newPolyBig, newPoly, sizeof(unsigned short)*nv);
+					delete newPoly;
+					newPoly = newPolyBig;			
+				}
+				newPoly[nv++] = dtris[tri*3*2+edge];
+				//move to next edge					
+				edge = (edge+1)%3;
+			}
+			else
+			{
+				//move to next tri
+				int twinedge = -1;
+				for (int k=0; k<3; k++)
+				{
+					if (dtris[neighbortri*3*2+3+k] == tri)
+					{
+						twinedge = k;
+						break;
+					}
+				}
+				if (twinedge==-1)
+				{
+					printf("Converting navmesh: Error! Can't find neighbor edge - invalid adjacency info\n");
+					goto returnLabel;					
+				}
+				tri = neighbortri;
+				edge = (twinedge+1)%3;
+			}
+		}
+
+		unsigned short* adjustedPoly = new unsigned short[nv];
+		int adjustedNv = 0;
+		for (size_t i=0; i<(size_t)nv; i++)
+		{
+			unsigned short prev = newPoly[(nv+i-1)%nv];
+			unsigned short cur = newPoly[i];
+			unsigned short next = newPoly[(i+1)%nv];
+			float distSq = distPointToSegmentSq(&verts[3*cur], &verts[3*prev], &verts[3*next]);
+			static const float tolerance = 0.001f;
+			if (distSq>tolerance)
+				adjustedPoly[adjustedNv++] = cur;
+		}
+		memcpy(newPoly, adjustedPoly, adjustedNv*sizeof(unsigned short));
+		delete adjustedPoly;
+		nv = adjustedNv;
+
+		if (nv<=vertsPerPoly)
+		{
+			for (int i=0; i<nv; i++)
+			{
+				polys[polyidx*vertsPerPoly*2+i] = newPoly[i];
+			}
+		}
+		else
+		{
+			int a=0;
+		}
+	}
+	res = true;
+
+returnLabel:
+	delete newPoly;
+	return true;
+}
+
+struct SortContext
+{
+	const int* recastData;
+	const int* trisToFacesMap;
+};
+static int compareByData(void* data, const void * a, const void * b){
+	SortContext* context = (SortContext*)data;
+	return ( context->recastData[context->trisToFacesMap[*(int*)a]] - 
+				context->recastData[context->trisToFacesMap[*(int*)b]] );
+}
+
+static bool buildNavMeshData(const int nverts, const float* verts, 
+							const int ntris, const unsigned short *tris, 
+							const int* recastData, const int* trisToFacesMap,
+							int &ndtris, unsigned short *&dtris,
+							int &npolys, unsigned short *&dmeshes, unsigned short *&polys,
+							int &vertsPerPoly, int *&dtrisToPolysMap, int *&dtrisToTrisMap)
+
+{
+	if (!recastData)
+	{
+		printf("Converting navmesh: Error! Can't find recast custom data\n");
+		return false;
+	}
+
+	//sort the triangles by polygon idx
+	int* trisMapping = new int[ntris];
+	for (int i=0; i<ntris; i++)
+		trisMapping[i]=i;
+	SortContext context;
+	context.recastData = recastData;
+	context.trisToFacesMap = trisToFacesMap;
+	qsort_s(trisMapping, ntris, sizeof(int), compareByData, &context);
+	
+	//search first valid triangle - triangle of convex polygon
+	int validTriStart = -1;
+	for (int i=0; i< ntris; i++)
+	{
+		if (recastData[trisToFacesMap[trisMapping[i]]]>0)
+		{
+			validTriStart = i;
+			break;
+		}
+	}
+
+	if (validTriStart<0)
+	{
+		printf("Converting navmesh: Error! No valid polygons in mesh\n");
+		delete trisMapping;
+		return false;
+	}
+
+	ndtris = ntris-validTriStart;
+	//fill dtris to faces mapping
+	dtrisToTrisMap = new int[ndtris];
+	memcpy(dtrisToTrisMap, &trisMapping[validTriStart], ndtris*sizeof(int));
+	delete trisMapping; trisMapping=NULL;
+
+	//create detailed mesh triangles  - copy only valid triangles
+	//and reserve memory for adjacency info
+	dtris = new unsigned short[3*2*ndtris];
+	memset(dtris, 0xffff, sizeof(unsigned short)*3*2*ndtris);
+	for (int i=0; i<ndtris; i++)
+	{
+		memcpy(dtris+3*2*i, tris+3*dtrisToTrisMap[i], sizeof(unsigned short)*3);
+	}
+	//create new recast data corresponded to dtris
+	dtrisToPolysMap = new int[ndtris];
+	for (int i=0; i<ndtris; i++)
+	{
+		dtrisToPolysMap[i] = recastData[trisToFacesMap[dtrisToTrisMap[i]]];
+	}
+	
+	
+	//build adjacency info for detailed mesh triangles
+	buildMeshAdjacency(dtris, ntris, nverts, 3);
+
+	//create detailed mesh description for each navigation polygon
+	npolys = dtrisToPolysMap[ndtris-1];
+	dmeshes = new unsigned short[npolys*4];
+	memset(dmeshes, 0, npolys*4*sizeof(unsigned short));
+	unsigned short *dmesh = NULL;
+	int prevpolyidx = 0;
+	for (int i=0; i<ndtris; i++)
+	{
+		int curpolyidx = dtrisToPolysMap[i];
+		if (curpolyidx!=prevpolyidx)
+		{
+			if (curpolyidx!=prevpolyidx+1)
+			{
+				printf("Converting navmesh: Error! Wrong order of detailed mesh faces\n");
+				return false;
+			}
+			dmesh = dmesh==NULL ? dmeshes : dmesh+4;
+			dmesh[2] = i;	//tbase
+			dmesh[3] = 0;	//tnum
+			prevpolyidx = curpolyidx;
+		}
+		dmesh[3]++;
+	}
+
+	//create navigation polygons
+	vertsPerPoly = 6;
+	polys = new unsigned short[npolys*vertsPerPoly*2];
+	memset(polys, 0xff, sizeof(unsigned short)*vertsPerPoly*2*npolys);
+
+	buildPolygonsByDetailedMeshes(vertsPerPoly, npolys, polys, dmeshes, verts, dtris, dtrisToPolysMap);
+
+	return true;
+}
+
+
+static bool buildNavMeshDataByDerivedMesh(DerivedMesh *dm, int& vertsPerPoly, 
+											int &nverts, float *&verts,
+											int &ndtris, unsigned short *&dtris,
+											int& npolys, unsigned short *&dmeshes,
+											unsigned short*& polys, int *&dtrisToPolysMap,
+											int *&dtrisToTrisMap, int *&trisToFacesMap)
+{
+	bool res = true;
+	int ntris =0, *recastData=NULL;
+	unsigned short *tris=NULL;
+	res = buildRawVertIndicesData(dm, nverts, verts, ntris, tris, trisToFacesMap, recastData);
+	if (!res)
+	{
+		printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
+		goto exit;
+	}
+
+	res = buildNavMeshData(nverts, verts, ntris, tris, recastData, trisToFacesMap,
+							ndtris, dtris, npolys, dmeshes,polys, vertsPerPoly, 
+							dtrisToPolysMap, dtrisToTrisMap);
+	if (!res)
+	{
+		printf("Converting navmesh: Error! Can't get vertices and indices from mesh\n");
+		goto exit;
+	}
+
+exit:
+	if (tris)
+		delete tris;
+
+	return res;
+}
+
+
+static void initData(ModifierData *md)
+{
+	NavMeshModifierData *nmmd = (NavMeshModifierData*) md;
+}
+
+static void copyData(ModifierData *md, ModifierData *target)
+{
+	NavMeshModifierData *nmmd = (NavMeshModifierData*) md;
+	NavMeshModifierData *tnmmd = (NavMeshModifierData*) target;
+
+	//.todo - deep copy
+}
+
 /*
 static void (*drawFacesSolid_original)(DerivedMesh *dm, float (*partial_redraw_planes)[4],
 					   int fast, int (*setMaterial)(int, void *attribs)) = NULL;*/
@@ -82,6 +454,7 @@ static void drawNavMeshColored(DerivedMesh *dm)
 	int* polygonIdx = (int*)CustomData_get_layer(&dm->faceData, CD_PROP_INT);
 	if (!polygonIdx)
 		return;
+	const float BLACK_COLOR[3] = {0.f, 0.f, 0.f};
 	float col[3];
 	/*
 	//UI_ThemeColor(TH_WIRE);
@@ -98,8 +471,11 @@ static void drawNavMeshColored(DerivedMesh *dm)
 		glBegin(glmode = GL_QUADS);
 		for(a = 0; a < dm->numFaceData; a++, mface++) {
 			int new_glmode = mface->v4?GL_QUADS:GL_TRIANGLES;
-			int* polygonIdx = (int*)CustomData_get(&dm->faceData, a, CD_PROP_INT);
-			intToCol(*polygonIdx, col);
+			int polygonIdx = *(int*)CustomData_get(&dm->faceData, a, CD_PROP_INT);
+			if (polygonIdx<=0)
+				memcpy(col, BLACK_COLOR, 3*sizeof(float));
+			else
+				intToCol(polygonIdx, col);
 
 			if(new_glmode != glmode) {
 				glEnd();
@@ -133,7 +509,6 @@ static void navDM_drawFacesSolid(DerivedMesh *dm,
 
 static DerivedMesh *createNavMeshForVisualization(NavMeshModifierData *mmd,DerivedMesh *dm)
 {
-	int i;
 	DerivedMesh *result;
 	int numVerts, numEdges, numFaces;
 	int maxVerts = dm->getNumVerts(dm);
@@ -141,47 +516,66 @@ static DerivedMesh *createNavMeshForVisualization(NavMeshModifierData *mmd,Deriv
 	int maxFaces = dm->getNumFaces(dm);
 
 	result = CDDM_copy(dm);
+	int *recastData = (int*)CustomData_get_layer(&dm->faceData, CD_PROP_INT);
 	CustomData_add_layer_named(&result->faceData, CD_PROP_INT, CD_DUPLICATE, 
-			CustomData_get_layer(&dm->faceData, CD_PROP_INT), maxFaces, "recastData");
-	
-	/*result = CDDM_new(maxVerts, maxEdges, maxFaces);
-	DM_copy_vert_data(dm, result, 0, 0, maxVerts);
-	DM_copy_edge_data(dm, result, 0, 0, maxEdges);
-	DM_copy_face_data(dm, result, 0, 0, maxFaces);*/
-
-	
-	/*
-	if (!drawFacesSolid_original)
-		drawFacesSolid_original= result->drawFacesSolid;*/
+			recastData, maxFaces, "recastData");
+	recastData = (int*)CustomData_get_layer(&result->faceData, CD_PROP_INT);
 	result->drawFacesTex =  navDM_drawFacesTex;
 	result->drawFacesSolid = navDM_drawFacesSolid;
 	
-/*
-	numVerts = numEdges = numFaces = 0;
-	for(i = 0; i < maxVerts; i++) {
-		MVert inMV;
-		MVert *mv = CDDM_get_vert(result, numVerts);
-		float co[3];
-		dm->getVert(dm, i, &inMV);
-		copy_v3_v3(co, inMV.co);
-		*mv = inMV;
-		//mv->co[2] +=.5f;
-		numVerts++;
+	
+	//process mesh
+	int vertsPerPoly=0, nverts=0, ndtris=0, npolys=0; 
+	float* verts=NULL;
+	unsigned short *dtris=NULL, *dmeshes=NULL, *polys=NULL;
+	int *dtrisToPolysMap=NULL, *dtrisToTrisMap=NULL, *trisToFacesMap=NULL;
+
+	bool res  = buildNavMeshDataByDerivedMesh(dm, vertsPerPoly, nverts, verts, ndtris, dtris,
+										npolys, dmeshes, polys, dtrisToPolysMap, dtrisToTrisMap,
+										trisToFacesMap);
+	if (res)
+	{
+		//invalidate concave polygon
+		for (size_t polyIdx=0; polyIdx<(size_t)npolys; polyIdx++)
+		{
+			unsigned short* poly = &polys[polyIdx*2*vertsPerPoly];
+			if (!polyIsConvex(poly, vertsPerPoly, verts))
+			{
+				//set negative polygon idx to all faces
+				unsigned short *dmesh = &dmeshes[4*polyIdx];
+				unsigned short tbase = dmesh[2];
+				unsigned short tnum = dmesh[3];
+				for (unsigned short ti=0; ti<tnum; ti++)
+				{
+					unsigned short triidx = dtrisToTrisMap[tbase+ti];
+					unsigned short faceidx = trisToFacesMap[triidx];
+					if (recastData[triidx]>0)
+						recastData[triidx] = -recastData[triidx];
+				}				
+			}
+		}
+
 	}
-	for(i = 0; i < maxEdges; i++) {
-		MEdge inMED;
-		MEdge *med = CDDM_get_edge(result, numEdges);
-		dm->getEdge(dm, i, &inMED);
-		*med = inMED;
-		numEdges++;
+	else
+	{
+		printf("Error during creation polygon infos\n");
 	}
-	for(i = 0; i < maxFaces; i++) {
-		MFace inMF;
-		MFace *mf = CDDM_get_face(result, numFaces);
-		dm->getFace(dm, i, &inMF);
-		*mf = inMF;
-		numFaces++;
-	}*/
+
+	//clean up
+	if (verts!=NULL)
+		delete verts;
+	if (dtris!=NULL)
+		delete dtris;
+	if (dmeshes!=NULL)
+		delete dmeshes;
+	if (polys!=NULL)
+		delete polys;
+	if (dtrisToPolysMap!=NULL)
+		delete dtrisToPolysMap;
+	if (dtrisToTrisMap!=NULL)
+		delete dtrisToTrisMap;	
+	if (trisToFacesMap!=NULL)
+		delete trisToFacesMap;		
 
 	return result;
 }
diff --git a/source/gameengine/Ketsji/KX_NavMeshObject.cpp b/source/gameengine/Ketsji/KX_NavMeshObject.cpp
index 19e045a2745..3d4dcb928a6 100644
--- a/source/gameengine/Ketsji/KX_NavMeshObject.cpp
+++ b/source/gameengine/Ketsji/KX_NavMeshObject.cpp
@@ -120,16 +120,11 @@ void KX_NavMeshObject::ProcessReplica()
 
 }
 
-bool KX_NavMeshObject::BuildVertIndArrays(RAS_MeshObject* meshobj, float *&vertices, int& nverts,
+bool KX_NavMeshObject::BuildVertIndArrays(float *&vertices, int& nverts,
 									   unsigned short* &polys, int& npolys, unsigned short *&dmeshes,
 									   float *&dvertices, int &ndvertsuniq, unsigned short *&dtris, 
 									   int& ndtris, int &vertsPerPoly)
 {
-	if (!meshobj) 
-	{
-		return false;
-	}
-
 	DerivedMesh* dm = mesh_create_derived_no_virtual(KX_GetActiveScene()->GetBlenderScene(), GetBlenderObject(), 
 													NULL, CD_MASK_MESH);
 	int* recastData = (int*) dm->getFaceDataArray(dm, CD_PROP_INT);
@@ -163,7 +158,7 @@ bool KX_NavMeshObject::BuildVertIndArrays(RAS_MeshObject* meshobj, float *&verti
 
 
 		//assumption: detailed mesh triangles are sorted by polygon idx
-		npolys = recastData[numfaces-1] + 1;
+		npolys = recastData[numfaces-1]/* + 1*/; //stored indices start from 1
 		
 		dmeshes = new unsigned short[npolys*4];
 		memset(dmeshes, 0, npolys*4*sizeof(unsigned short));
@@ -177,6 +172,7 @@ bool KX_NavMeshObject::BuildVertIndArrays(RAS_MeshObject* meshobj, float *&verti
 				if (curpolyidx!=prevpolyidx+1)
 				{
 					//error - wrong order of detailed mesh faces
+					printf("Converting navmesh: Error! Wrong order of detailed mesh faces\n");
 					return false;
 				}
 				dmesh = dmesh==NULL ? dmeshes : dmesh+4;
@@ -386,6 +382,7 @@ bool KX_NavMeshObject::BuildVertIndArrays(RAS_MeshObject* meshobj, float *&verti
 	else
 	{
 		//create from RAS_MeshObject (detailed mesh is fake)
+		RAS_MeshObject* meshobj = GetMesh(0);
 		vertsPerPoly = 3;
 		nverts = meshobj->m_sharedvertex_map.size();
 		if (nverts >= 0xffff)
@@ -452,20 +449,23 @@ bool KX_NavMeshObject::BuildNavMesh()
 		m_navMesh = NULL;
 	}
 
-
 	if (GetMeshCount()==0)
+	{
+		printf("Can't find mesh for navmesh object: %s \n", m_name);
 		return false;
-
-	RAS_MeshObject* meshobj = GetMesh(0);
+	}
 
 	float *vertices = NULL, *dvertices = NULL;
 	unsigned short *polys = NULL, *dtris = NULL, *dmeshes = NULL;
 	int nverts = 0, npolys = 0, ndvertsuniq = 0, ndtris = 0;	
 	int vertsPerPoly = 0;
-	if (!BuildVertIndArrays(meshobj, vertices, nverts, polys, npolys, 
+	if (!BuildVertIndArrays(vertices, nverts, polys, npolys, 
 							dmeshes, dvertices, ndvertsuniq, dtris, ndtris, vertsPerPoly ) 
 			|| vertsPerPoly<3)
+	{
+		printf("Can't build navigation mesh data for object:%s \n", m_name);
 		return false;
+	}
 	
 	MT_Point3 pos;
 	for (int i=0; i<nverts; i++)
diff --git a/source/gameengine/Ketsji/KX_NavMeshObject.h b/source/gameengine/Ketsji/KX_NavMeshObject.h
index 5161b723844..a0b91804db1 100644
--- a/source/gameengine/Ketsji/KX_NavMeshObject.h
+++ b/source/gameengine/Ketsji/KX_NavMeshObject.h
@@ -43,7 +43,7 @@ class KX_NavMeshObject: public KX_GameObject
 protected:
 	dtStatNavMesh* m_navMesh;
 	
-	bool BuildVertIndArrays(RAS_MeshObject* meshobj, float *&vertices, int& nverts,
+	bool BuildVertIndArrays(float *&vertices, int& nverts,
 							unsigned short* &polys, int& npolys, unsigned short *&dmeshes, 
 							float *&dvertices, int &ndvertsuniq, unsigned short* &dtris, 
 							int& ndtris, int &vertsPerPoly);