diff options
author | Nick Samarin <nicks1987@bigmir.net> | 2010-08-04 23:32:37 +0400 |
---|---|---|
committer | Nick Samarin <nicks1987@bigmir.net> | 2010-08-04 23:32:37 +0400 |
commit | a2372308d739879e99423851bc1e1dcd82cf19bf (patch) | |
tree | cc3bb7217585e77f8506f7a030f1f51e8b9c677e | |
parent | dbc8d4274fa47f429084f653008a14980432a65d (diff) |
integrated adaptive sampling algorithm for obstacle avoidance
6 files changed, 359 insertions, 631 deletions
diff --git a/extern/recastnavigation/BlenderNavMesh/NavMeshConversion.cpp b/extern/recastnavigation/BlenderNavMesh/NavMeshConversion.cpp deleted file mode 100644 index ca11ed68c54..00000000000 --- a/extern/recastnavigation/BlenderNavMesh/NavMeshConversion.cpp +++ /dev/null @@ -1,413 +0,0 @@ -/** -* $Id$ -* -* ***** BEGIN GPL LICENSE BLOCK ***** -* -* This program is free software; you can redistribute it and/or -* modify it under the terms of the GNU General Public License -* as published by the Free Software Foundation; either version 2 -* of the License, or (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software Foundation, -* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. -* -* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. -* All rights reserved. -* -* The Original Code is: all of this file. -* -* Contributor(s): none yet. -* -* ***** END GPL LICENSE BLOCK ***** -*/ - -#include "NavMeshConversion.h" -extern "C"{ -#include "BLI_math.h" -} - -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; -} - -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; -} - -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]; -} - -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->v3; - 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; -} - -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]] ); -} - -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 and renumber for continious indices - int prevPolyIdx=-1, curPolyIdx, newPolyIdx=0; - dtrisToPolysMap = new int[ndtris]; - for (int i=0; i<ndtris; i++) - { - curPolyIdx = recastData[trisToFacesMap[dtrisToTrisMap[i]]]; - if (curPolyIdx!=prevPolyIdx) - { - newPolyIdx++; - prevPolyIdx=curPolyIdx; - } - dtrisToPolysMap[i] = newPolyIdx; - } - - - //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; -} - - -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; -} - -int polyFindVertex(const unsigned short* p, const int vertsPerPoly, unsigned short vertexIdx) -{ - int res = -1; - for(int i=0; i<vertsPerPoly; i++) - { - if (p[i]==0xffff) - break; - if (p[i]==vertexIdx) - { - res = i; - break; - } - } - return res; -}
\ No newline at end of file diff --git a/extern/recastnavigation/BlenderNavMesh/NavMeshConversion.h b/extern/recastnavigation/BlenderNavMesh/NavMeshConversion.h deleted file mode 100644 index 5a30d927c7f..00000000000 --- a/extern/recastnavigation/BlenderNavMesh/NavMeshConversion.h +++ /dev/null @@ -1,98 +0,0 @@ -/** -* $Id$ -* -* ***** BEGIN GPL LICENSE BLOCK ***** -* -* This program is free software; you can redistribute it and/or -* modify it under the terms of the GNU General Public License -* as published by the Free Software Foundation; either version 2 -* of the License, or (at your option) any later version. -* -* This program is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with this program; if not, write to the Free Software Foundation, -* Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA. -* -* The Original Code is Copyright (C) 2001-2002 by NaN Holding BV. -* All rights reserved. -* -* The Original Code is: all of this file. -* -* Contributor(s): none yet. -* -* ***** END GPL LICENSE BLOCK ***** -*/ - -#ifndef NAVMESH_CONVERSION_H -#define NAVMESH_CONVERSION_H - -#include <math.h> -#include "Recast.h" -extern "C"{ -#include "DNA_meshdata_types.h" -#include "BKE_cdderivedmesh.h" -} - -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 buildRawVertIndicesData(DerivedMesh* dm, int &nverts, float *&verts, - int &ntris, unsigned short *&tris, int *&trisToFacesMap, - int *&recastData); - -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); - -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); - -int polyNumVerts(const unsigned short* p, const int vertsPerPoly); -bool polyIsConvex(const unsigned short* p, const int vertsPerPoly, const float* verts); -int polyFindVertex(const unsigned short* p, const int vertsPerPoly, unsigned short vertexIdx); -float distPointToSegmentSq(const float* point, const float* a, const float* b); - - -inline int abs2(int a) -{ - return a>=0 ? a: -a; -} - -inline int bit(int a, int b) -{ - return (a & (1 << b)) >> b; -} - -inline void intToCol(int i, float* col) -{ - int r = bit(i, 0) + bit(i, 3) * 2 + 1; - int g = bit(i, 1) + bit(i, 4) * 2 + 1; - int b = bit(i, 2) + bit(i, 5) * 2 + 1; - col[0] = 1 - r*63.0f/255.0f; - 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; -} - -#endif //NAVMESH_CONVERSION_H
\ No newline at end of file diff --git a/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj b/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj index 37401df6323..ab013aebc21 100644 --- a/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj +++ b/projectfiles_vc9/gameengine/ketsji/KX_ketsji.vcproj @@ -271,7 +271,7 @@ Name="VCCLCompilerTool"
Optimization="2"
InlineFunctionExpansion="1"
- AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\build\msvc_9\extern\recastnavigation\Detour\Include;..\..\..\..\build\msvc_9\extern\recastnavigation\Recast\Include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\sdl\include;..\..\..\source\blender\imbuf;..\..\..\source\blender\include;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\python\generic;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenloader;..\..\..\source\blender\blenkernel;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\Converter;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\scenegraph;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\physics\sumo\include;..\..\..\source\gameengine\physics\common\dummy;..\..\..\source\gameengine\Rasterizer\RAS_OpenGLRasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\sumo\include;..\..\..\source\sumo\fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\python\api2_2x;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern"
+ AdditionalIncludeDirectories="..\..\..\..\build\msvc_9\intern\string\include;..\..\..\..\build\msvc_9\intern\moto\include;..\..\..\..\build\msvc_9\intern\soundsystem\include;..\..\..\..\build\msvc_9\intern\guardedalloc\include;..\..\..\..\build\msvc_9\extern\bullet\include;..\..\..\..\build\msvc_9\extern\solid\include;..\..\..\..\build\msvc_9\extern\recastnavigation\Detour\Include;..\..\..\..\build\msvc_9\extern\recastnavigation\Recast\Include;..\..\..\..\build\msvc_9\extern\glew\include;..\..\..\..\lib\windows\python\include\python3.1;..\..\..\..\lib\windows\sdl\include;..\..\..\source\blender\imbuf;..\..\..\source\blender\include;..\..\..\source\blender\blenlib;..\..\..\source\blender\python;..\..\..\source\blender\python\generic;..\..\..\source\blender\makesdna;..\..\..\source\blender\blenloader;..\..\..\source\blender\blenkernel;..\..\..\source\blender\editors\include;..\..\..\source\kernel\gen_system;..\..\..\source\gameengine\physics;..\..\..\source\gameengine\rasterizer;..\..\..\source\gameengine\network;..\..\..\source\gameengine\Converter;..\..\..\source\gameengine\gamelogic;..\..\..\source\gameengine\scenegraph;..\..\..\source\gameengine\expressions;..\..\..\source\gameengine\physics\sumo;..\..\..\source\gameengine\physics\dummy;..\..\..\source\gameengine\physics\BlOde;..\..\..\source\gameengine\ketsji\kxnetwork;..\..\..\source\gameengine\physics\common;..\..\..\source\gameengine\physics\sumo\include;..\..\..\source\gameengine\physics\common\dummy;..\..\..\source\gameengine\Rasterizer\RAS_OpenGLRasterizer;..\..\..\source\gameengine\physics\sumo\fuzzics\include;..\..\..\source\sumo\include;..\..\..\source\sumo\fuzzics\include;..\..\..\source\gameengine\physics\bullet;..\..\..\source\blender\python\api2_2x;..\..\..\source\blender\gpu;..\..\..\intern\audaspace\intern"
PreprocessorDefinitions="NDEBUG;WIN32;_LIB;USE_SUMO_SOLID;WITH_GLEXT;GLEW_STATIC;WITH_FFMPEG"
StringPooling="true"
RuntimeLibrary="0"
diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp index dcaadd23e3f..4a1ea59e12c 100644 --- a/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp +++ b/source/gameengine/Ketsji/KX_ObstacleSimulation.cpp @@ -38,8 +38,65 @@ #include "DNA_object_types.h" #include "BLI_math.h" -inline float perp(const MT_Vector2& a, const MT_Vector2& b) { return a.x()*b.y() - a.y()*b.x(); } -inline float lerp(float a, float b, float t) { return a + (b-a)*t; } +namespace +{ + inline float perp(const MT_Vector2& a, const MT_Vector2& b) { return a.x()*b.y() - a.y()*b.x(); } + + inline float sqr(float x) { return x*x; } + inline float lerp(float a, float b, float t) { return a + (b-a)*t; } + inline float clamp(float a, float mn, float mx) { return a < mn ? mn : (a > mx ? mx : a); } + + inline float vdistsqr(const float* a, const float* b) { return sqr(b[0]-a[0]) + sqr(b[1]-a[1]); } + inline float vdist(const float* a, const float* b) { return sqrtf(vdistsqr(a,b)); } + inline void vcpy(float* a, const float* b) { a[0]=b[0]; a[1]=b[1]; } + inline float vdot(const float* a, const float* b) { return a[0]*b[0] + a[1]*b[1]; } + inline float vperp(const float* a, const float* b) { return a[0]*b[1] - a[1]*b[0]; } + inline void vsub(float* v, const float* a, const float* b) { v[0] = a[0]-b[0]; v[1] = a[1]-b[1]; } + inline void vadd(float* v, const float* a, const float* b) { v[0] = a[0]+b[0]; v[1] = a[1]+b[1]; } + inline void vscale(float* v, const float* a, const float s) { v[0] = a[0]*s; v[1] = a[1]*s; } + inline void vset(float* v, float x, float y) { v[0]=x; v[1]=y; } + inline float vlensqr(const float* v) { return vdot(v,v); } + inline float vlen(const float* v) { return sqrtf(vlensqr(v)); } + inline void vlerp(float* v, const float* a, const float* b, float t) { v[0] = lerp(a[0], b[0], t); v[1] = lerp(a[1], b[1], t); } + inline void vmad(float* v, const float* a, const float* b, float s) { v[0] = a[0] + b[0]*s; v[1] = a[1] + b[1]*s; } + inline void vnorm(float* v) + { + float d = vlen(v); + if (d > 0.0001f) + { + d = 1.0f/d; + v[0] *= d; + v[1] *= d; + } + } +} +inline float triarea(const float* a, const float* b, const float* c) +{ + return (b[0]*a[1] - a[0]*b[1]) + (c[0]*b[1] - b[0]*c[1]) + (a[0]*c[1] - c[0]*a[1]); +} + +static void closestPtPtSeg(const float* pt, + const float* sp, const float* sq, + float& t) +{ + float dir[2],diff[3]; + vsub(dir,sq,sp); + vsub(diff,pt,sp); + t = vdot(diff,dir); + if (t <= 0.0f) { t = 0; return; } + float d = vdot(dir,dir); + if (t >= d) { t = 1; return; } + t /= d; +} + +static float distPtSegSqr(const float* pt, const float* sp, const float* sq) +{ + float t; + closestPtPtSeg(pt, sp,sq, t); + float np[2]; + vlerp(np, sp,sq, t); + return vdistsqr(pt,np); +} static int sweepCircleCircle(const MT_Vector3& pos0, const MT_Scalar r0, const MT_Vector2& v, const MT_Vector3& pos1, const MT_Scalar r1, @@ -152,20 +209,6 @@ static bool inBetweenAngle(float a, float amin, float amax, float& t) return false; } -static float interpolateToi(float a, const float* dir, const float* toi, const int ntoi) -{ - for (int i = 0; i < ntoi; ++i) - { - int next = (i+1) % ntoi; - float t; - if (inBetweenAngle(a, dir[i], dir[next], t)) - { - return lerp(toi[i], toi[next], t); - } - } - return 0; -} - KX_ObstacleSimulation::KX_ObstacleSimulation(MT_Scalar levelHeight, bool enableVisualization) : m_levelHeight(levelHeight) , m_enableVisualization(enableVisualization) @@ -186,6 +229,15 @@ KX_Obstacle* KX_ObstacleSimulation::CreateObstacle(KX_GameObject* gameobj) { KX_Obstacle* obstacle = new KX_Obstacle(); obstacle->m_gameObj = gameobj; + + vset(obstacle->vel, 0,0); + vset(obstacle->pvel, 0,0); + vset(obstacle->dvel, 0,0); + vset(obstacle->nvel, 0,0); + for (int i = 0; i < VEL_HIST_SIZE; ++i) + vset(&obstacle->hvel[i*2], 0,0); + obstacle->hhead = 0; + gameobj->RegisterObstacle(this); m_obstacles.push_back(obstacle); return obstacle; @@ -222,7 +274,6 @@ void KX_ObstacleSimulation::AddObstaclesForNavMesh(KX_NavMeshObject* navmeshobj) obstacle->m_pos = MT_Point3(vj[0], vj[2], vj[1]); obstacle->m_pos2 = MT_Point3(vi[0], vi[2], vi[1]); obstacle->m_rad = 0; - obstacle->m_vel = MT_Vector2(0,0); } } } @@ -254,8 +305,16 @@ void KX_ObstacleSimulation::UpdateObstacles() KX_Obstacle* obs = m_obstacles[i]; obs->m_pos = obs->m_gameObj->NodeGetWorldPosition(); - obs->m_vel.x() = obs->m_gameObj->GetLinearVelocity().x(); - obs->m_vel.y() = obs->m_gameObj->GetLinearVelocity().y(); + obs->vel[0] = obs->m_gameObj->GetLinearVelocity().x(); + obs->vel[1] = obs->m_gameObj->GetLinearVelocity().y(); + + // Update velocity history and calculate perceived (average) velocity. + vcpy(&obs->hvel[obs->hhead*2], obs->vel); + obs->hhead = (obs->hhead+1) % VEL_HIST_SIZE; + vset(obs->pvel,0,0); + for (int j = 0; j < VEL_HIST_SIZE; ++j) + vadd(obs->pvel, obs->pvel, &obs->hvel[j*2]); + vscale(obs->pvel, obs->pvel, 1.0f/VEL_HIST_SIZE); } } @@ -329,7 +388,8 @@ static MT_Point3 nearestPointToObstacle(MT_Point3& pos ,KX_Obstacle* obstacle) } } -bool KX_ObstacleSimulation::FilterObstacle(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, KX_Obstacle* otherObst) +static bool filterObstacle(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, KX_Obstacle* otherObst, + float levelHeight) { //filter obstacles by type if ( (otherObst == activeObst) || @@ -338,7 +398,7 @@ bool KX_ObstacleSimulation::FilterObstacle(KX_Obstacle* activeObst, KX_NavMeshOb //filter obstacles by position MT_Point3 p = nearestPointToObstacle(activeObst->m_pos, otherObst); - if ( fabs(activeObst->m_pos.z() - p.z()) > m_levelHeight) + if ( fabs(activeObst->m_pos.z() - p.z()) > levelHeight) return false; return true; @@ -373,71 +433,100 @@ KX_Obstacle* KX_ObstacleSimulationTOI::CreateObstacle(KX_GameObject* gameobj) return obstacle; } -void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, - MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle) -{ - int nobs = m_obstacles.size(); - int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin(); - if (obstidx == nobs) - return; - TOICircle* tc = m_toiCircles[obstidx]; +static const float VEL_WEIGHT = 2.0f; +static const float CUR_VEL_WEIGHT = 0.75f; +static const float SIDE_WEIGHT = 0.75f; +static const float TOI_WEIGHT = 2.5f; - MT_Vector2 vel(velocity.x(), velocity.y()); - float vmax = (float) velocity.length(); - float odir = (float) atan2(velocity.y(), velocity.x()); +static void processSamples(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, + KX_Obstacles& obstacles, float levelHeight, const float vmax, + const float* spos, const float cs, const int nspos, + float* res) +{ + vset(res, 0,0); - MT_Vector2 ddir = vel; - ddir.normalize(); + const float ivmax = 1.0f / vmax; - float bestScore = FLT_MAX; - float bestDir = odir; - float bestToi = 0; + // Max time of collision to be considered. + const float maxToi = 1.5f; - tc->n = m_avoidSteps; - tc->minToi = m_minToi; - tc->maxToi = m_maxToi; + float adir[2], adist; + vcpy(adir, activeObst->pvel); + if (vlen(adir) > 0.01f) + vnorm(adir); + else + vset(adir,0,0); + float activeObstPos[2]; + vset(activeObstPos, activeObst->m_pos.x(), activeObst->m_pos.y()); + adist = vdot(adir, activeObstPos); - const int iforw = m_avoidSteps/2; - const float aoff = (float)iforw / (float)m_avoidSteps; + float minPenalty = FLT_MAX; - for (int iter = 0; iter < m_avoidSteps; ++iter) + for (int n = 0; n < nspos; ++n) { - // Calculate sample velocity - const float ndir = ((float)iter/(float)m_avoidSteps) - aoff; - const float dir = odir+ndir*M_PI*2; - MT_Vector2 svel; - svel.x() = cosf(dir) * vmax; - svel.y() = sinf(dir) * vmax; + float vcand[2]; + vcpy(vcand, &spos[n*2]); // Find min time of impact and exit amongst all obstacles. - float tmin = m_maxToi; - float tmine = 0; - for (int i = 0; i < nobs; ++i) + float tmin = maxToi; + float side = 0; + int nside = 0; + + for (int i = 0; i < obstacles.size(); ++i) { - KX_Obstacle* ob = m_obstacles[i]; - bool res = FilterObstacle(activeObst, activeNavMeshObj, ob); + KX_Obstacle* ob = obstacles[i]; + bool res = filterObstacle(activeObst, activeNavMeshObj, ob, levelHeight); if (!res) continue; - - float htmin,htmax; + float htmin, htmax; - if (ob->m_shape == KX_OBSTACLE_CIRCLE) + if (ob->m_shape==KX_OBSTACLE_CIRCLE) { - MT_Vector2 vab; - if (ob->m_vel.length2() < 0.01f*0.01f) + float vab[2]; + + // Moving, use RVO + vscale(vab, vcand, 2); + vsub(vab, vab, activeObst->vel); + vsub(vab, vab, ob->vel); + + // Side + // NOTE: dp, and dv are constant over the whole calculation, + // they can be precomputed per object. + const float* pa = activeObstPos; + float pb[2]; + vset(pb, ob->m_pos.x(), ob->m_pos.y()); + + const float orig[2] = {0,0}; + float dp[2],dv[2],np[2]; + vsub(dp,pb,pa); + vnorm(dp); + vsub(dv,ob->dvel, activeObst->dvel); + + const float a = triarea(orig, dp,dv); + if (a < 0.01f) { - // Stationary, use VO - vab = svel; + np[0] = -dp[1]; + np[1] = dp[0]; } else { - // Moving, use RVO - vab = 2*svel - vel - ob->m_vel; + np[0] = dp[1]; + np[1] = -dp[0]; } - if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, - vab, ob->m_pos, ob->m_rad, htmin, htmax)) + side += clamp(min(vdot(dp,vab)*2,vdot(np,vab)*2), 0.0f, 1.0f); + nside++; + + if (!sweepCircleCircle(activeObst->m_pos, activeObst->m_rad, vab, ob->m_pos, ob->m_rad, + htmin, htmax)) continue; + + // Handle overlapping obstacles. + if (htmin < 0.0f && htmax > 0.0f) + { + // Avoid more when overlapped. + htmin = -htmin * 0.5f; + } } else if (ob->m_shape == KX_OBSTACLE_SEGMENT) { @@ -450,77 +539,193 @@ void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, K p1 = navmeshobj->TransformToWorldCoords(p1); p2 = navmeshobj->TransformToWorldCoords(p2); } - if (!sweepCircleSegment(activeObst->m_pos, activeObst->m_rad, svel, - p1, p2, ob->m_rad, htmin, htmax)) - continue; + float p[2], q[2]; + vset(p, p1.x(), p1.y()); + vset(q, p2.x(), p2.y()); + + // NOTE: the segments are assumed to come from a navmesh which is shrunken by + // the agent radius, hence the use of really small radius. + // This can be handle more efficiently by using seg-seg test instead. + // If the whole segment is to be treated as obstacle, use agent->rad instead of 0.01f! + const float r = 0.01f; // agent->rad + if (distPtSegSqr(activeObstPos, p, q) < sqr(r+ob->m_rad)) + { + float sdir[2], snorm[2]; + vsub(sdir, q, p); + snorm[0] = sdir[1]; + snorm[1] = -sdir[0]; + // If the velocity is pointing towards the segment, no collision. + if (vdot(snorm, vcand) < 0.0f) + continue; + // Else immediate collision. + htmin = 0.0f; + htmax = 10.0f; + } + else + { + if (!sweepCircleSegment(activeObstPos, r, vcand, p, q, ob->m_rad, htmin, htmax)) + continue; + } + + // Avoid less when facing walls. + htmin *= 2.0f; } - if (htmin > 0.0f) + if (htmin >= 0.0f) { // The closest obstacle is somewhere ahead of us, keep track of nearest obstacle. if (htmin < tmin) tmin = htmin; } - else if (htmax > 0.0f) - { - // The agent overlaps the obstacle, keep track of first safe exit. - if (htmax > tmine) - tmine = htmax; - } } - // Calculate sample penalties and final score. - const float apen = m_angleWeight * fabsf(ndir); - const float tpen = m_toiWeight * (1.0f/(0.0001f+tmin/m_maxToi)); - const float cpen = m_collisionWeight * (tmine/m_minToi)*(tmine/m_minToi); - const float score = apen + tpen + cpen; + // Normalize side bias, to prevent it dominating too much. + if (nside) + side /= nside; + + const float vpen = VEL_WEIGHT * (vdist(vcand, activeObst->dvel) * ivmax); + const float vcpen = CUR_VEL_WEIGHT * (vdist(vcand, activeObst->vel) * ivmax); + const float spen = SIDE_WEIGHT * side; + const float tpen = TOI_WEIGHT * (1.0f/(0.1f+tmin/maxToi)); - // Update best score. - if (score < bestScore) + const float penalty = vpen + vcpen + spen + tpen; + + if (penalty < minPenalty) { - bestDir = dir; - bestToi = tmin; - bestScore = score; + minPenalty = penalty; + vcpy(res, vcand); } - - tc->dir[iter] = dir; - tc->toi[iter] = tmin; - tc->toie[iter] = tmine; } +} + +static const int RVO_SAMPLE_RAD = 15; +static const int MAX_RVO_SAMPLES = (RVO_SAMPLE_RAD*2+1)*(RVO_SAMPLE_RAD*2+1) + 100; + +static void sampleRVO(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, + KX_Obstacles& obstacles, const float levelHeight,const float bias) +{ + float spos[2*MAX_RVO_SAMPLES]; + int nspos = 0; + + const float cvx = activeObst->dvel[0]*bias; + const float cvy = activeObst->dvel[1]*bias; + float vmax = vlen(activeObst->dvel); + const float vrange = vmax*(1-bias); + const float cs = 1.0f / (float)RVO_SAMPLE_RAD*vrange; - if (activeObst->m_vel.length() > 0.1) + for (int y = -RVO_SAMPLE_RAD; y <= RVO_SAMPLE_RAD; ++y) { - // Constrain max turn rate. - float cura = atan2(activeObst->m_vel.y(),activeObst->m_vel.x()); - float da = bestDir - cura; - if (da < -M_PI) da += (float)M_PI*2; - if (da > M_PI) da -= (float)M_PI*2; - if (da < -maxDeltaAngle) + for (int x = -RVO_SAMPLE_RAD; x <= RVO_SAMPLE_RAD; ++x) { - bestDir = cura - maxDeltaAngle; - bestToi = min(bestToi, interpolateToi(bestDir, tc->dir, tc->toi, tc->n)); + if (nspos < MAX_RVO_SAMPLES) + { + const float vx = cvx + (float)(x+0.5f)*cs; + const float vy = cvy + (float)(y+0.5f)*cs; + if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue; + spos[nspos*2+0] = vx; + spos[nspos*2+1] = vy; + nspos++; + } } - else if (da > maxDeltaAngle) + } + processSamples(activeObst, activeNavMeshObj, obstacles, levelHeight, vmax, spos, cs/2, + nspos, activeObst->nvel); +} + +static void sampleRVOAdaptive(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, + KX_Obstacles& obstacles, const float levelHeight,const float bias) +{ + vset(activeObst->nvel, 0.f, 0.f); + float vmax = vlen(activeObst->dvel); + + float spos[2*MAX_RVO_SAMPLES]; + int nspos = 0; + + int rad; + float res[2]; + float cs; + + // First sample location. + rad = 4; + res[0] = activeObst->dvel[0]*bias; + res[1] = activeObst->dvel[1]*bias; + cs = vmax*(2-bias*2) / (float)(rad-1); + + for (int k = 0; k < 5; ++k) + { + const float half = (rad-1)*cs*0.5f; + + nspos = 0; + for (int y = 0; y < rad; ++y) { - bestDir = cura + maxDeltaAngle; - bestToi = min(bestToi, interpolateToi(bestDir, tc->dir, tc->toi, tc->n)); + for (int x = 0; x < rad; ++x) + { + const float vx = res[0] + x*cs - half; + const float vy = res[1] + y*cs - half; + if (vx*vx+vy*vy > sqr(vmax+cs/2)) continue; + spos[nspos*2+0] = vx; + spos[nspos*2+1] = vy; + nspos++; + } } + + processSamples(activeObst, activeNavMeshObj, obstacles, levelHeight, vmax, spos, cs/2, + nspos, res); + + cs *= 0.5f; } - // Adjust speed when time of impact is less than min TOI. - if (bestToi < m_minToi) - vmax *= bestToi/m_minToi; + vcpy(activeObst->nvel, res); +} - // Constrain velocity change. - const float curSpeed = (float) activeObst->m_vel.length(); - float deltaSpeed = vmax - curSpeed; - CLAMP(deltaSpeed, -maxDeltaSpeed, maxDeltaSpeed); - vmax = curSpeed + deltaSpeed; - // New steering velocity. - vel.x() = cosf(bestDir) * vmax; - vel.y() = sinf(bestDir) * vmax; +void KX_ObstacleSimulationTOI::AdjustObstacleVelocity(KX_Obstacle* activeObst, KX_NavMeshObject* activeNavMeshObj, + MT_Vector3& velocity, MT_Scalar maxDeltaSpeed, MT_Scalar maxDeltaAngle) +{ + int nobs = m_obstacles.size(); + int obstidx = std::find(m_obstacles.begin(), m_obstacles.end(), activeObst) - m_obstacles.begin(); + if (obstidx == nobs) + return; + + vset(activeObst->dvel, velocity.x(), velocity.y()); + + //apply RVO + const float bias = 0.4f; + //sampleRVO(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, bias); + sampleRVOAdaptive(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, bias); + + // Fake dynamic constraint. + float dv[2]; + float vel[2]; + vsub(dv, activeObst->nvel, activeObst->vel); + float ds = vlen(dv); + if (ds > maxDeltaSpeed || ds<-maxDeltaSpeed) + vscale(dv, dv, fabs(maxDeltaSpeed/ds)); + vadd(vel, activeObst->vel, dv); + + velocity.x() = vel[0]; + velocity.y() = vel[1]; +/* printf("dvel: %f, nvel: %f, vel: %f\n", vlen(activeObst->dvel), vlen(activeObst->nvel), + vlen(vel));*/ - velocity.x() = vel.x(); - velocity.y() = vel.y(); -}
\ No newline at end of file +} +/* +#include "GL/glew.h" +void KX_ObstacleSimulation::DebugDraw() +{ + glDisable(GL_LIGHTING); + glDisable(GL_TEXTURE_2D); + glMatrixMode(GL_MODELVIEW); + glLoadIdentity(); + glMatrixMode(GL_PROJECTION); + glLoadIdentity(); + glOrtho(0.0, 100.0, 0.0, 100.0, -1.0, 1.0); + glBegin(GL_QUADS); + glColor4ub(255,0,0,255); + glVertex2f(0.f, 0.f); + glVertex2f(100.f, 25.f); + glVertex2f(100.f, 75.f); + glVertex2f(25.f, 75.f); + glEnd(); + +}*/
\ No newline at end of file diff --git a/source/gameengine/Ketsji/KX_ObstacleSimulation.h b/source/gameengine/Ketsji/KX_ObstacleSimulation.h index b084f54d3b8..559c4f8caf8 100644 --- a/source/gameengine/Ketsji/KX_ObstacleSimulation.h +++ b/source/gameengine/Ketsji/KX_ObstacleSimulation.h @@ -56,6 +56,7 @@ enum KX_OBSTACLE_SHAPE KX_OBSTACLE_SEGMENT, }; +#define VEL_HIST_SIZE 6 struct KX_Obstacle { KX_OBSTACLE_TYPE m_type; @@ -63,26 +64,50 @@ struct KX_Obstacle MT_Point3 m_pos; MT_Point3 m_pos2; MT_Scalar m_rad; - MT_Vector2 m_vel; + + float vel[2]; + float pvel[2]; + float dvel[2]; + float nvel[2]; + float hvel[VEL_HIST_SIZE*2]; + int hhead; + KX_GameObject* m_gameObj; }; +typedef std::vector<KX_Obstacle*> KX_Obstacles; +/* +struct RVO +{ + inline RVO() : ns(0) {} + float spos[MAX_RVO_SAMPLES*2]; + float scs[MAX_RVO_SAMPLES]; + float spen[MAX_RVO_SAMPLES]; + + float svpen[MAX_RVO_SAMPLES]; + float svcpen[MAX_RVO_SAMPLES]; + float sspen[MAX_RVO_SAMPLES]; + float stpen[MAX_RVO_SAMPLES]; + + int ns; +}; +*/ class KX_ObstacleSimulation { protected: - std::vector<KX_Obstacle*> m_obstacles; + KX_Obstacles m_obstacles; MT_Scalar m_levelHeight; bool m_enableVisualization; virtual KX_Obstacle* CreateObstacle(KX_GameObject* gameobj); - bool FilterObstacle(KX_Obstacle* activeObstacle, KX_NavMeshObject* activeNavMeshObj, KX_Obstacle* otherObstacle); public: KX_ObstacleSimulation(MT_Scalar levelHeight, bool enableVisualization); virtual ~KX_ObstacleSimulation(); void DrawObstacles(); + //void DebugDraw(); void AddObstacleForObj(KX_GameObject* gameobj); void DestroyObstacleForObj(KX_GameObject* gameobj); diff --git a/source/gameengine/Ketsji/KX_SteeringActuator.cpp b/source/gameengine/Ketsji/KX_SteeringActuator.cpp index f70826d16f9..e093ddb4072 100644 --- a/source/gameengine/Ketsji/KX_SteeringActuator.cpp +++ b/source/gameengine/Ketsji/KX_SteeringActuator.cpp @@ -254,7 +254,7 @@ bool KX_SteeringActuator::Update(double curtime, bool frame) MT_Vector3 newvel = m_velocity*steervec; //adjust velocity to avoid obstacles - if (m_simulation && m_obstacle && !newvel.fuzzyZero()) + if (m_simulation && m_obstacle /*&& !newvel.fuzzyZero()*/) { if (m_enableVisualization) KX_RasterizerDrawDebugLine(mypos, mypos + newvel, MT_Vector3(1.,0.,0.)); @@ -278,6 +278,15 @@ bool KX_SteeringActuator::Update(double curtime, bool frame) obj->ApplyMovement(movement, false); } } + else + { + if (m_simulation && m_obstacle) + { + m_obstacle->dvel[0] = 0.f; + m_obstacle->dvel[1] = 0.f; + } + + } if (terminate && m_isSelfTerminated) return false; |