integrated adaptive sampling algorithm for obstacle avoidance
This commit is contained in:
Nick Samarin
@@ -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;
|
||||
}
|
||||
@@ -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
|
||||
@@ -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"
|
||||
|
||||
@@ -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)
|
||||
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;
|
||||
|
||||
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)
|
||||
{
|
||||
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];
|
||||
vset(res, 0,0);
|
||||
|
||||
MT_Vector2 vel(velocity.x(), velocity.y());
|
||||
float vmax = (float) velocity.length();
|
||||
float odir = (float) atan2(velocity.y(), velocity.x());
|
||||
const float ivmax = 1.0f / vmax;
|
||||
|
||||
MT_Vector2 ddir = vel;
|
||||
ddir.normalize();
|
||||
// Max time of collision to be considered.
|
||||
const float maxToi = 1.5f;
|
||||
|
||||
float bestScore = FLT_MAX;
|
||||
float bestDir = odir;
|
||||
float bestToi = 0;
|
||||
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);
|
||||
|
||||
tc->n = m_avoidSteps;
|
||||
tc->minToi = m_minToi;
|
||||
tc->maxToi = m_maxToi;
|
||||
float minPenalty = FLT_MAX;
|
||||
|
||||
const int iforw = m_avoidSteps/2;
|
||||
const float aoff = (float)iforw / (float)m_avoidSteps;
|
||||
|
||||
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)
|
||||
}
|
||||
|
||||
// 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));
|
||||
|
||||
const float penalty = vpen + vcpen + spen + tpen;
|
||||
|
||||
if (penalty < minPenalty)
|
||||
{
|
||||
minPenalty = penalty;
|
||||
vcpy(res, vcand);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
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;
|
||||
|
||||
for (int y = -RVO_SAMPLE_RAD; y <= RVO_SAMPLE_RAD; ++y)
|
||||
{
|
||||
for (int x = -RVO_SAMPLE_RAD; x <= RVO_SAMPLE_RAD; ++x)
|
||||
{
|
||||
if (nspos < MAX_RVO_SAMPLES)
|
||||
{
|
||||
// The agent overlaps the obstacle, keep track of first safe exit.
|
||||
if (htmax > tmine)
|
||||
tmine = htmax;
|
||||
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++;
|
||||
}
|
||||
}
|
||||
}
|
||||
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)
|
||||
{
|
||||
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++;
|
||||
}
|
||||
}
|
||||
|
||||
// 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;
|
||||
processSamples(activeObst, activeNavMeshObj, obstacles, levelHeight, vmax, spos, cs/2,
|
||||
nspos, res);
|
||||
|
||||
// Update best score.
|
||||
if (score < bestScore)
|
||||
{
|
||||
bestDir = dir;
|
||||
bestToi = tmin;
|
||||
bestScore = score;
|
||||
}
|
||||
|
||||
tc->dir[iter] = dir;
|
||||
tc->toi[iter] = tmin;
|
||||
tc->toie[iter] = tmine;
|
||||
cs *= 0.5f;
|
||||
}
|
||||
|
||||
if (activeObst->m_vel.length() > 0.1)
|
||||
{
|
||||
// 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)
|
||||
{
|
||||
bestDir = cura - maxDeltaAngle;
|
||||
bestToi = min(bestToi, interpolateToi(bestDir, tc->dir, tc->toi, tc->n));
|
||||
}
|
||||
else if (da > maxDeltaAngle)
|
||||
{
|
||||
bestDir = cura + maxDeltaAngle;
|
||||
bestToi = min(bestToi, interpolateToi(bestDir, tc->dir, tc->toi, tc->n));
|
||||
}
|
||||
}
|
||||
vcpy(activeObst->nvel, res);
|
||||
}
|
||||
|
||||
// Adjust speed when time of impact is less than min TOI.
|
||||
if (bestToi < m_minToi)
|
||||
vmax *= bestToi/m_minToi;
|
||||
|
||||
// Constrain velocity change.
|
||||
const float curSpeed = (float) activeObst->m_vel.length();
|
||||
float deltaSpeed = vmax - curSpeed;
|
||||
CLAMP(deltaSpeed, -maxDeltaSpeed, maxDeltaSpeed);
|
||||
vmax = curSpeed + deltaSpeed;
|
||||
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());
|
||||
|
||||
// New steering velocity.
|
||||
vel.x() = cosf(bestDir) * vmax;
|
||||
vel.y() = sinf(bestDir) * vmax;
|
||||
//apply RVO
|
||||
const float bias = 0.4f;
|
||||
//sampleRVO(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, bias);
|
||||
sampleRVOAdaptive(activeObst, activeNavMeshObj, m_obstacles, m_levelHeight, bias);
|
||||
|
||||
velocity.x() = vel.x();
|
||||
velocity.y() = vel.y();
|
||||
}
|
||||
// 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));*/
|
||||
|
||||
}
|
||||
/*
|
||||
#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();
|
||||
|
||||
}*/
|
||||
@@ -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);
|
||||
|
||||
@@ -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;
|
||||
|
||||
Reference in New Issue
Block a user