Relintai/pandemonium_engine_minimal
1
0
Fork 0
mirror of https://github.com/Relintai/pandemonium_engine_minimal.git synced 2025-03-23 19:57:33 +01:00
Code Issues Packages Projects Releases Wiki Activity

Removed recast and rvo2.

Browse Source
This commit is contained in:
Relintai 2023-12-16 00:03:39 +01:00
parent cf46fb8237
commit 710b4d8bcf
34 changed files with 0 additions and 15197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
thirdparty/recastnavigation/License.txt vendored
View File

@ -1,18 +0,0 @@
Copyright (c) 2009 Mikko Mononen memon@inside.org
This software is provided 'as-is', without any express or implied
warranty. In no event will the authors be held liable for any damages
arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it
freely, subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not
claim that you wrote the original software. If you use this software
in a product, an acknowledgment in the product documentation would be
appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be
misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.

1336
thirdparty/recastnavigation/Recast/Include/Recast.h vendored
View File

File diff suppressed because it is too large Load Diff

372
thirdparty/recastnavigation/Recast/Include/RecastAlloc.h vendored
View File

@ -1,372 +0,0 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#ifndef RECASTALLOC_H
#define RECASTALLOC_H
#include "RecastAssert.h"
#include <stdlib.h>
#include <stdint.h>
/// Provides hint values to the memory allocator on how long the
/// memory is expected to be used.
enum rcAllocHint
{
RC_ALLOC_PERM, ///< Memory will persist after a function call.
RC_ALLOC_TEMP ///< Memory used temporarily within a function.
};
/// A memory allocation function.
// @param[in] size The size, in bytes of memory, to allocate.
// @param[in] rcAllocHint A hint to the allocator on how long the memory is expected to be in use.
// @return A pointer to the beginning of the allocated memory block, or null if the allocation failed.
/// @see rcAllocSetCustom
typedef void* (rcAllocFunc)(size_t size, rcAllocHint hint);
/// A memory deallocation function.
/// @param[in] ptr A pointer to a memory block previously allocated using #rcAllocFunc.
/// @see rcAllocSetCustom
typedef void (rcFreeFunc)(void* ptr);
/// Sets the base custom allocation functions to be used by Recast.
/// @param[in] allocFunc The memory allocation function to be used by #rcAlloc
/// @param[in] freeFunc The memory de-allocation function to be used by #rcFree
///
/// @see rcAlloc, rcFree
void rcAllocSetCustom(rcAllocFunc *allocFunc, rcFreeFunc *freeFunc);
/// Allocates a memory block.
///
/// @param[in] size The size, in bytes of memory, to allocate.
/// @param[in] hint A hint to the allocator on how long the memory is expected to be in use.
/// @return A pointer to the beginning of the allocated memory block, or null if the allocation failed.
///
/// @see rcFree, rcAllocSetCustom
void* rcAlloc(size_t size, rcAllocHint hint);
/// Deallocates a memory block. If @p ptr is NULL, this does nothing.
///
/// @warning This function leaves the value of @p ptr unchanged. So it still
/// points to the same (now invalid) location, and not to null.
///
/// @param[in] ptr A pointer to a memory block previously allocated using #rcAlloc.
///
/// @see rcAlloc, rcAllocSetCustom
void rcFree(void* ptr);
/// An implementation of operator new usable for placement new. The default one is part of STL (which we don't use).
/// rcNewTag is a dummy type used to differentiate our operator from the STL one, in case users import both Recast
/// and STL.
struct rcNewTag {};
inline void* operator new(size_t, const rcNewTag&, void* p) { return p; }
inline void operator delete(void*, const rcNewTag&, void*) {}
/// Signed to avoid warnnings when comparing to int loop indexes, and common error with comparing to zero.
/// MSVC2010 has a bug where ssize_t is unsigned (!!!).
typedef intptr_t rcSizeType;
#define RC_SIZE_MAX INTPTR_MAX
/// Macros to hint to the compiler about the likeliest branch. Please add a benchmark that demonstrates a performance
/// improvement before introducing use cases.
#if defined(__GNUC__) || defined(__clang__)
#define rcLikely(x) __builtin_expect((x), true)
#define rcUnlikely(x) __builtin_expect((x), false)
#else
#define rcLikely(x) (x)
#define rcUnlikely(x) (x)
#endif
/// Variable-sized storage type. Mimics the interface of std::vector<T> with some notable differences:
/// * Uses rcAlloc()/rcFree() to handle storage.
/// * No support for a custom allocator.
/// * Uses signed size instead of size_t to avoid warnings in for loops: "for (int i = 0; i < foo.size(); i++)"
/// * Omits methods of limited utility: insert/erase, (bad performance), at (we don't use exceptions), operator=.
/// * assign() and the pre-sizing constructor follow C++11 semantics -- they don't construct a temporary if no value is provided.
/// * push_back() and resize() support adding values from the current vector. Range-based constructors and assign(begin, end) do not.
/// * No specialization for bool.
template <typename T, rcAllocHint H>
class rcVectorBase {
rcSizeType m_size;
rcSizeType m_cap;
T* m_data;
// Constructs a T at the give address with either the copy constructor or the default.
static void construct(T* p, const T& v) { ::new(rcNewTag(), (void*)p) T(v); }
static void construct(T* p) { ::new(rcNewTag(), (void*)p) T; }
static void construct_range(T* begin, T* end);
static void construct_range(T* begin, T* end, const T& value);
static void copy_range(T* dst, const T* begin, const T* end);
void destroy_range(rcSizeType begin, rcSizeType end);
// Creates an array of the given size, copies all of this vector's data into it, and returns it.
T* allocate_and_copy(rcSizeType size);
void resize_impl(rcSizeType size, const T* value);
// Requires: min_capacity > m_cap.
rcSizeType get_new_capacity(rcSizeType min_capacity);
public:
typedef rcSizeType size_type;
typedef T value_type;
rcVectorBase() : m_size(0), m_cap(0), m_data(0) {}
rcVectorBase(const rcVectorBase<T, H>& other) : m_size(0), m_cap(0), m_data(0) { assign(other.begin(), other.end()); }
explicit rcVectorBase(rcSizeType count) : m_size(0), m_cap(0), m_data(0) { resize(count); }
rcVectorBase(rcSizeType count, const T& value) : m_size(0), m_cap(0), m_data(0) { resize(count, value); }
rcVectorBase(const T* begin, const T* end) : m_size(0), m_cap(0), m_data(0) { assign(begin, end); }
~rcVectorBase() { destroy_range(0, m_size); rcFree(m_data); }
// Unlike in std::vector, we return a bool to indicate whether the alloc was successful.
bool reserve(rcSizeType size);
void assign(rcSizeType count, const T& value) { clear(); resize(count, value); }
void assign(const T* begin, const T* end);
void resize(rcSizeType size) { resize_impl(size, NULL); }
void resize(rcSizeType size, const T& value) { resize_impl(size, &value); }
// Not implemented as resize(0) because resize requires T to be default-constructible.
void clear() { destroy_range(0, m_size); m_size = 0; }
void push_back(const T& value);
void pop_back() { rcAssert(m_size > 0); back().~T(); m_size--; }
rcSizeType size() const { return m_size; }
rcSizeType capacity() const { return m_cap; }
bool empty() const { return size() == 0; }
const T& operator[](rcSizeType i) const { rcAssert(i >= 0 && i < m_size); return m_data[i]; }
T& operator[](rcSizeType i) { rcAssert(i >= 0 && i < m_size); return m_data[i]; }
const T& front() const { rcAssert(m_size); return m_data[0]; }
T& front() { rcAssert(m_size); return m_data[0]; }
const T& back() const { rcAssert(m_size); return m_data[m_size - 1]; }
T& back() { rcAssert(m_size); return m_data[m_size - 1]; }
const T* data() const { return m_data; }
T* data() { return m_data; }
T* begin() { return m_data; }
T* end() { return m_data + m_size; }
const T* begin() const { return m_data; }
const T* end() const { return m_data + m_size; }
void swap(rcVectorBase<T, H>& other);
// Explicitly deleted.
rcVectorBase& operator=(const rcVectorBase<T, H>& other);
};
template<typename T, rcAllocHint H>
bool rcVectorBase<T, H>::reserve(rcSizeType count) {
if (count <= m_cap) {
return true;
}
T* new_data = allocate_and_copy(count);
if (!new_data) {
return false;
}
destroy_range(0, m_size);
rcFree(m_data);
m_data = new_data;
m_cap = count;
return true;
}
template <typename T, rcAllocHint H>
T* rcVectorBase<T, H>::allocate_and_copy(rcSizeType size) {
rcAssert(RC_SIZE_MAX / static_cast<rcSizeType>(sizeof(T)) >= size);
T* new_data = static_cast<T*>(rcAlloc(sizeof(T) * size, H));
if (new_data) {
copy_range(new_data, m_data, m_data + m_size);
}
return new_data;
}
template <typename T, rcAllocHint H>
void rcVectorBase<T, H>::assign(const T* begin, const T* end) {
clear();
reserve(end - begin);
m_size = end - begin;
copy_range(m_data, begin, end);
}
template <typename T, rcAllocHint H>
void rcVectorBase<T, H>::push_back(const T& value) {
// rcLikely increases performance by ~50% on BM_rcVector_PushPreallocated,
// and by ~2-5% on BM_rcVector_Push.
if (rcLikely(m_size < m_cap)) {
construct(m_data + m_size++, value);
return;
}
const rcSizeType new_cap = get_new_capacity(m_cap + 1);
T* data = allocate_and_copy(new_cap);
// construct between allocate and destroy+free in case value is
// in this vector.
construct(data + m_size, value);
destroy_range(0, m_size);
m_size++;
m_cap = new_cap;
rcFree(m_data);
m_data = data;
}
template <typename T, rcAllocHint H>
rcSizeType rcVectorBase<T, H>::get_new_capacity(rcSizeType min_capacity) {
rcAssert(min_capacity <= RC_SIZE_MAX);
if (rcUnlikely(m_cap >= RC_SIZE_MAX / 2))
return RC_SIZE_MAX;
return 2 * m_cap > min_capacity ? 2 * m_cap : min_capacity;
}
template <typename T, rcAllocHint H>
void rcVectorBase<T, H>::resize_impl(rcSizeType size, const T* value) {
if (size < m_size) {
destroy_range(size, m_size);
m_size = size;
} else if (size > m_size) {
if (size <= m_cap) {
if (value) {
construct_range(m_data + m_size, m_data + size, *value);
} else {
construct_range(m_data + m_size, m_data + size);
}
m_size = size;
} else {
const rcSizeType new_cap = get_new_capacity(size);
T* new_data = allocate_and_copy(new_cap);
// We defer deconstructing/freeing old data until after constructing
// new elements in case "value" is there.
if (value) {
construct_range(new_data + m_size, new_data + size, *value);
} else {
construct_range(new_data + m_size, new_data + size);
}
destroy_range(0, m_size);
rcFree(m_data);
m_data = new_data;
m_cap = new_cap;
m_size = size;
}
}
}
template <typename T, rcAllocHint H>
void rcVectorBase<T, H>::swap(rcVectorBase<T, H>& other) {
// TODO: Reorganize headers so we can use rcSwap here.
rcSizeType tmp_cap = other.m_cap;
rcSizeType tmp_size = other.m_size;
T* tmp_data = other.m_data;
other.m_cap = m_cap;
other.m_size = m_size;
other.m_data = m_data;
m_cap = tmp_cap;
m_size = tmp_size;
m_data = tmp_data;
}
// static
template <typename T, rcAllocHint H>
void rcVectorBase<T, H>::construct_range(T* begin, T* end) {
for (T* p = begin; p < end; p++) {
construct(p);
}
}
// static
template <typename T, rcAllocHint H>
void rcVectorBase<T, H>::construct_range(T* begin, T* end, const T& value) {
for (T* p = begin; p < end; p++) {
construct(p, value);
}
}
// static
template <typename T, rcAllocHint H>
void rcVectorBase<T, H>::copy_range(T* dst, const T* begin, const T* end) {
for (rcSizeType i = 0 ; i < end - begin; i++) {
construct(dst + i, begin[i]);
}
}
template <typename T, rcAllocHint H>
void rcVectorBase<T, H>::destroy_range(rcSizeType begin, rcSizeType end) {
for (rcSizeType i = begin; i < end; i++) {
m_data[i].~T();
}
}
template <typename T>
class rcTempVector : public rcVectorBase<T, RC_ALLOC_TEMP> {
typedef rcVectorBase<T, RC_ALLOC_TEMP> Base;
public:
rcTempVector() : Base() {}
explicit rcTempVector(rcSizeType size) : Base(size) {}
rcTempVector(rcSizeType size, const T& value) : Base(size, value) {}
rcTempVector(const rcTempVector<T>& other) : Base(other) {}
rcTempVector(const T* begin, const T* end) : Base(begin, end) {}
};
template <typename T>
class rcPermVector : public rcVectorBase<T, RC_ALLOC_PERM> {
typedef rcVectorBase<T, RC_ALLOC_PERM> Base;
public:
rcPermVector() : Base() {}
explicit rcPermVector(rcSizeType size) : Base(size) {}
rcPermVector(rcSizeType size, const T& value) : Base(size, value) {}
rcPermVector(const rcPermVector<T>& other) : Base(other) {}
rcPermVector(const T* begin, const T* end) : Base(begin, end) {}
};
/// Legacy class. Prefer rcVector<int>.
class rcIntArray
{
rcTempVector<int> m_impl;
public:
rcIntArray() {}
rcIntArray(int n) : m_impl(n, 0) {}
void push(int item) { m_impl.push_back(item); }
void resize(int size) { m_impl.resize(size); }
void clear() { m_impl.clear(); }
int pop()
{
int v = m_impl.back();
m_impl.pop_back();
return v;
}
int size() const { return static_cast<int>(m_impl.size()); }
int& operator[](int index) { return m_impl[index]; }
int operator[](int index) const { return m_impl[index]; }
};
/// A simple helper class used to delete an array when it goes out of scope.
/// @note This class is rarely if ever used by the end user.
template<class T> class rcScopedDelete
{
T* ptr;
public:
/// Constructs an instance with a null pointer.
inline rcScopedDelete() : ptr(0) {}
/// Constructs an instance with the specified pointer.
/// @param[in] p An pointer to an allocated array.
inline rcScopedDelete(T* p) : ptr(p) {}
inline ~rcScopedDelete() { rcFree(ptr); }
/// The root array pointer.
/// @return The root array pointer.
inline operator T*() { return ptr; }
private:
// Explicitly disabled copy constructor and copy assignment operator.
rcScopedDelete(const rcScopedDelete&);
rcScopedDelete& operator=(const rcScopedDelete&);
};
#endif

53
thirdparty/recastnavigation/Recast/Include/RecastAssert.h vendored
View File

@ -1,53 +0,0 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#ifndef RECASTASSERT_H
#define RECASTASSERT_H
#ifdef NDEBUG
// From https://web.archive.org/web/20210117002833/http://cnicholson.net/2009/02/stupid-c-tricks-adventures-in-assert/
# define rcAssert(x) do { (void)sizeof(x); } while ((void)(__LINE__==-1), false)
#else
/// An assertion failure function.
// @param[in] expression asserted expression.
// @param[in] file Filename of the failed assertion.
// @param[in] line Line number of the failed assertion.
/// @see rcAssertFailSetCustom
typedef void (rcAssertFailFunc)(const char* expression, const char* file, int line);
/// Sets the base custom assertion failure function to be used by Recast.
/// @param[in] assertFailFunc The function to be used in case of failure of #dtAssert
void rcAssertFailSetCustom(rcAssertFailFunc* assertFailFunc);
/// Gets the base custom assertion failure function to be used by Recast.
rcAssertFailFunc* rcAssertFailGetCustom();
# include <assert.h>
# define rcAssert(expression) \
{ \
rcAssertFailFunc* failFunc = rcAssertFailGetCustom(); \
if (failFunc == NULL) { assert(expression); } \
else if (!(expression)) { (*failFunc)(#expression, __FILE__, __LINE__); } \
}
#endif
#endif // RECASTASSERT_H

542
thirdparty/recastnavigation/Recast/Source/Recast.cpp vendored
View File

@ -1,542 +0,0 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include "Recast.h"
#include "RecastAlloc.h"
#include "RecastAssert.h"
#include <math.h>
#include <string.h>
#include <stdio.h>
#include <stdarg.h>
namespace
{
/// Allocates and constructs an object of the given type, returning a pointer.
/// @param[in] allocLifetime Allocation lifetime hint
template<typename T>
T* rcNew(const rcAllocHint allocLifetime)
{
T* ptr = (T*)rcAlloc(sizeof(T), allocLifetime);
::new(rcNewTag(), (void*)ptr) T();
return ptr;
}
/// Destroys and frees an object allocated with rcNew.
/// @param[in] ptr The object pointer to delete.
template<typename T>
void rcDelete(T* ptr)
{
if (ptr)
{
ptr->~T();
rcFree((void*)ptr);
}
}
} // anonymous namespace
float rcSqrt(float x)
{
return sqrtf(x);
}
void rcContext::log(const rcLogCategory category, const char* format, ...)
{
if (!m_logEnabled)
{
return;
}
static const int MSG_SIZE = 512;
char msg[MSG_SIZE];
va_list argList;
va_start(argList, format);
int len = vsnprintf(msg, MSG_SIZE, format, argList);
if (len >= MSG_SIZE)
{
len = MSG_SIZE - 1;
msg[MSG_SIZE - 1] = '\0';
const char* errorMessage = "Log message was truncated";
doLog(RC_LOG_ERROR, errorMessage, (int)strlen(errorMessage));
}
va_end(argList);
doLog(category, msg, len);
}
void rcContext::doResetLog()
{
// Defined out of line to fix the weak v-tables warning
}
rcHeightfield* rcAllocHeightfield()
{
return rcNew<rcHeightfield>(RC_ALLOC_PERM);
}
void rcFreeHeightField(rcHeightfield* heightfield)
{
rcDelete(heightfield);
}
rcHeightfield::rcHeightfield()
: width()
, height()
, bmin()
, bmax()
, cs()
, ch()
, spans()
, pools()
, freelist()
{
}
rcHeightfield::~rcHeightfield()
{
// Delete span array.
rcFree(spans);
// Delete span pools.
while (pools)
{
rcSpanPool* next = pools->next;
rcFree(pools);
pools = next;
}
}
rcCompactHeightfield* rcAllocCompactHeightfield()
{
return rcNew<rcCompactHeightfield>(RC_ALLOC_PERM);
}
void rcFreeCompactHeightfield(rcCompactHeightfield* compactHeightfield)
{
rcDelete(compactHeightfield);
}
rcCompactHeightfield::rcCompactHeightfield()
: width()
, height()
, spanCount()
, walkableHeight()
, walkableClimb()
, borderSize()
, maxDistance()
, maxRegions()
, bmin()
, bmax()
, cs()
, ch()
, cells()
, spans()
, dist()
, areas()
{
}
rcCompactHeightfield::~rcCompactHeightfield()
{
rcFree(cells);
rcFree(spans);
rcFree(dist);
rcFree(areas);
}
rcHeightfieldLayerSet* rcAllocHeightfieldLayerSet()
{
return rcNew<rcHeightfieldLayerSet>(RC_ALLOC_PERM);
}
void rcFreeHeightfieldLayerSet(rcHeightfieldLayerSet* layerSet)
{
rcDelete(layerSet);
}
rcHeightfieldLayerSet::rcHeightfieldLayerSet()
: layers()
, nlayers()
{
}
rcHeightfieldLayerSet::~rcHeightfieldLayerSet()
{
for (int i = 0; i < nlayers; ++i)
{
rcFree(layers[i].heights);
rcFree(layers[i].areas);
rcFree(layers[i].cons);
}
rcFree(layers);
}
rcContourSet* rcAllocContourSet()
{
return rcNew<rcContourSet>(RC_ALLOC_PERM);
}
void rcFreeContourSet(rcContourSet* contourSet)
{
rcDelete(contourSet);
}
rcContourSet::rcContourSet()
: conts()
, nconts()
, bmin()
, bmax()
, cs()
, ch()
, width()
, height()
, borderSize()
, maxError()
{
}
rcContourSet::~rcContourSet()
{
for (int i = 0; i < nconts; ++i)
{
rcFree(conts[i].verts);
rcFree(conts[i].rverts);
}
rcFree(conts);
}
rcPolyMesh* rcAllocPolyMesh()
{
return rcNew<rcPolyMesh>(RC_ALLOC_PERM);
}
void rcFreePolyMesh(rcPolyMesh* polyMesh)
{
rcDelete(polyMesh);
}
rcPolyMesh::rcPolyMesh()
: verts()
, polys()
, regs()
, flags()
, areas()
, nverts()
, npolys()
, maxpolys()
, nvp()
, bmin()
, bmax()
, cs()
, ch()
, borderSize()
, maxEdgeError()
{
}
rcPolyMesh::~rcPolyMesh()
{
rcFree(verts);
rcFree(polys);
rcFree(regs);
rcFree(flags);
rcFree(areas);
}
rcPolyMeshDetail* rcAllocPolyMeshDetail()
{
return rcNew<rcPolyMeshDetail>(RC_ALLOC_PERM);
}
void rcFreePolyMeshDetail(rcPolyMeshDetail* detailMesh)
{
if (detailMesh == NULL)
{
return;
}
rcFree(detailMesh->meshes);
rcFree(detailMesh->verts);
rcFree(detailMesh->tris);
rcFree(detailMesh);
}
rcPolyMeshDetail::rcPolyMeshDetail()
: meshes()
, verts()
, tris()
, nmeshes()
, nverts()
, ntris()
{
}
void rcCalcBounds(const float* verts, int numVerts, float* minBounds, float* maxBounds)
{
// Calculate bounding box.
rcVcopy(minBounds, verts);
rcVcopy(maxBounds, verts);
for (int i = 1; i < numVerts; ++i)
{
const float* v = &verts[i * 3];
rcVmin(minBounds, v);
rcVmax(maxBounds, v);
}
}
void rcCalcGridSize(const float* minBounds, const float* maxBounds, const float cellSize, int* sizeX, int* sizeZ)
{
*sizeX = (int)((maxBounds[0] - minBounds[0]) / cellSize + 0.5f);
*sizeZ = (int)((maxBounds[2] - minBounds[2]) / cellSize + 0.5f);
}
bool rcCreateHeightfield(rcContext* context, rcHeightfield& heightfield, int sizeX, int sizeZ,
const float* minBounds, const float* maxBounds,
float cellSize, float cellHeight)
{
rcIgnoreUnused(context);
heightfield.width = sizeX;
heightfield.height = sizeZ;
rcVcopy(heightfield.bmin, minBounds);
rcVcopy(heightfield.bmax, maxBounds);
heightfield.cs = cellSize;
heightfield.ch = cellHeight;
heightfield.spans = (rcSpan**)rcAlloc(sizeof(rcSpan*) * heightfield.width * heightfield.height, RC_ALLOC_PERM);
if (!heightfield.spans)
{
return false;
}
memset(heightfield.spans, 0, sizeof(rcSpan*) * heightfield.width * heightfield.height);
return true;
}
static void calcTriNormal(const float* v0, const float* v1, const float* v2, float* faceNormal)
{
float e0[3], e1[3];
rcVsub(e0, v1, v0);
rcVsub(e1, v2, v0);
rcVcross(faceNormal, e0, e1);
rcVnormalize(faceNormal);
}
void rcMarkWalkableTriangles(rcContext* context, const float walkableSlopeAngle,
const float* verts, const int numVerts,
const int* tris, const int numTris,
unsigned char* triAreaIDs)
{
rcIgnoreUnused(context);
rcIgnoreUnused(numVerts);
const float walkableThr = cosf(walkableSlopeAngle / 180.0f * RC_PI);
float norm[3];
for (int i = 0; i < numTris; ++i)
{
const int* tri = &tris[i * 3];
calcTriNormal(&verts[tri[0] * 3], &verts[tri[1] * 3], &verts[tri[2] * 3], norm);
// Check if the face is walkable.
if (norm[1] > walkableThr)
{
triAreaIDs[i] = RC_WALKABLE_AREA;
}
}
}
void rcClearUnwalkableTriangles(rcContext* context, const float walkableSlopeAngle,
const float* verts, int numVerts,
const int* tris, int numTris,
unsigned char* triAreaIDs)
{
rcIgnoreUnused(context);
rcIgnoreUnused(numVerts);
// The minimum Y value for a face normal of a triangle with a walkable slope.
const float walkableLimitY = cosf(walkableSlopeAngle / 180.0f * RC_PI);
float faceNormal[3];
for (int i = 0; i < numTris; ++i)
{
const int* tri = &tris[i * 3];
calcTriNormal(&verts[tri[0] * 3], &verts[tri[1] * 3], &verts[tri[2] * 3], faceNormal);
// Check if the face is walkable.
if (faceNormal[1] <= walkableLimitY)
{
triAreaIDs[i] = RC_NULL_AREA;
}
}
}
int rcGetHeightFieldSpanCount(rcContext* context, const rcHeightfield& heightfield)
{
rcIgnoreUnused(context);
const int numCols = heightfield.width * heightfield.height;
int spanCount = 0;
for (int columnIndex = 0; columnIndex < numCols; ++columnIndex)
{
for (rcSpan* span = heightfield.spans[columnIndex]; span != NULL; span = span->next)
{
if (span->area != RC_NULL_AREA)
{
spanCount++;
}
}
}
return spanCount;
}
bool rcBuildCompactHeightfield(rcContext* context, const int walkableHeight, const int walkableClimb,
const rcHeightfield& heightfield, rcCompactHeightfield& compactHeightfield)
{
rcAssert(context);
rcScopedTimer timer(context, RC_TIMER_BUILD_COMPACTHEIGHTFIELD);
const int xSize = heightfield.width;
const int zSize = heightfield.height;
const int spanCount = rcGetHeightFieldSpanCount(context, heightfield);
// Fill in header.
compactHeightfield.width = xSize;
compactHeightfield.height = zSize;
compactHeightfield.spanCount = spanCount;
compactHeightfield.walkableHeight = walkableHeight;
compactHeightfield.walkableClimb = walkableClimb;
compactHeightfield.maxRegions = 0;
rcVcopy(compactHeightfield.bmin, heightfield.bmin);
rcVcopy(compactHeightfield.bmax, heightfield.bmax);
compactHeightfield.bmax[1] += walkableHeight * heightfield.ch;
compactHeightfield.cs = heightfield.cs;
compactHeightfield.ch = heightfield.ch;
compactHeightfield.cells = (rcCompactCell*)rcAlloc(sizeof(rcCompactCell) * xSize * zSize, RC_ALLOC_PERM);
if (!compactHeightfield.cells)
{
context->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.cells' (%d)", xSize * zSize);
return false;
}
memset(compactHeightfield.cells, 0, sizeof(rcCompactCell) * xSize * zSize);
compactHeightfield.spans = (rcCompactSpan*)rcAlloc(sizeof(rcCompactSpan) * spanCount, RC_ALLOC_PERM);
if (!compactHeightfield.spans)
{
context->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.spans' (%d)", spanCount);
return false;
}
memset(compactHeightfield.spans, 0, sizeof(rcCompactSpan) * spanCount);
compactHeightfield.areas = (unsigned char*)rcAlloc(sizeof(unsigned char) * spanCount, RC_ALLOC_PERM);
if (!compactHeightfield.areas)
{
context->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Out of memory 'chf.areas' (%d)", spanCount);
return false;
}
memset(compactHeightfield.areas, RC_NULL_AREA, sizeof(unsigned char) * spanCount);
const int MAX_HEIGHT = 0xffff;
// Fill in cells and spans.
int currentCellIndex = 0;
const int numColumns = xSize * zSize;
for (int columnIndex = 0; columnIndex < numColumns; ++columnIndex)
{
const rcSpan* span = heightfield.spans[columnIndex];
// If there are no spans at this cell, just leave the data to index=0, count=0.
if (span == NULL)
{
continue;
}
rcCompactCell& cell = compactHeightfield.cells[columnIndex];
cell.index = currentCellIndex;
cell.count = 0;
for (; span != NULL; span = span->next)
{
if (span->area != RC_NULL_AREA)
{
const int bot = (int)span->smax;
const int top = span->next ? (int)span->next->smin : MAX_HEIGHT;
compactHeightfield.spans[currentCellIndex].y = (unsigned short)rcClamp(bot, 0, 0xffff);
compactHeightfield.spans[currentCellIndex].h = (unsigned char)rcClamp(top - bot, 0, 0xff);
compactHeightfield.areas[currentCellIndex] = span->area;
currentCellIndex++;
cell.count++;
}
}
}
// Find neighbour connections.
const int MAX_LAYERS = RC_NOT_CONNECTED - 1;
int maxLayerIndex = 0;
const int zStride = xSize; // for readability
for (int z = 0; z < zSize; ++z)
{
for (int x = 0; x < xSize; ++x)
{
const rcCompactCell& cell = compactHeightfield.cells[x + z * zStride];
for (int i = (int)cell.index, ni = (int)(cell.index + cell.count); i < ni; ++i)
{
rcCompactSpan& span = compactHeightfield.spans[i];
for (int dir = 0; dir < 4; ++dir)
{
rcSetCon(span, dir, RC_NOT_CONNECTED);
const int neighborX = x + rcGetDirOffsetX(dir);
const int neighborZ = z + rcGetDirOffsetY(dir);
// First check that the neighbour cell is in bounds.
if (neighborX < 0 || neighborZ < 0 || neighborX >= xSize || neighborZ >= zSize)
{
continue;
}
// Iterate over all neighbour spans and check if any of the is
// accessible from current cell.
const rcCompactCell& neighborCell = compactHeightfield.cells[neighborX + neighborZ * zStride];
for (int k = (int)neighborCell.index, nk = (int)(neighborCell.index + neighborCell.count); k < nk; ++k)
{
const rcCompactSpan& neighborSpan = compactHeightfield.spans[k];
const int bot = rcMax(span.y, neighborSpan.y);
const int top = rcMin(span.y + span.h, neighborSpan.y + neighborSpan.h);
// Check that the gap between the spans is walkable,
// and that the climb height between the gaps is not too high.
if ((top - bot) >= walkableHeight && rcAbs((int)neighborSpan.y - (int)span.y) <= walkableClimb)
{
// Mark direction as walkable.
const int layerIndex = k - (int)neighborCell.index;
if (layerIndex < 0 || layerIndex > MAX_LAYERS)
{
maxLayerIndex = rcMax(maxLayerIndex, layerIndex);
continue;
}
rcSetCon(span, dir, layerIndex);
break;
}
}
}
}
}
}
if (maxLayerIndex > MAX_LAYERS)
{
context->log(RC_LOG_ERROR, "rcBuildCompactHeightfield: Heightfield has too many layers %d (max: %d)",
maxLayerIndex, MAX_LAYERS);
}
return true;
}

51
thirdparty/recastnavigation/Recast/Source/RecastAlloc.cpp vendored
View File

@ -1,51 +0,0 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include "RecastAlloc.h"
static void* rcAllocDefault(size_t size, rcAllocHint)
{
return malloc(size);
}
static void rcFreeDefault(void *ptr)
{
free(ptr);
}
static rcAllocFunc* sRecastAllocFunc = rcAllocDefault;
static rcFreeFunc* sRecastFreeFunc = rcFreeDefault;
void rcAllocSetCustom(rcAllocFunc* allocFunc, rcFreeFunc* freeFunc)
{
sRecastAllocFunc = allocFunc ? allocFunc : rcAllocDefault;
sRecastFreeFunc = freeFunc ? freeFunc : rcFreeDefault;
}
void* rcAlloc(size_t size, rcAllocHint hint)
{
return sRecastAllocFunc(size, hint);
}
void rcFree(void* ptr)
{
if (ptr != NULL)
{
sRecastFreeFunc(ptr);
}
}

590
thirdparty/recastnavigation/Recast/Source/RecastArea.cpp vendored
View File

@ -1,590 +0,0 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include <float.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include "Recast.h"
#include "RecastAlloc.h"
#include "RecastAssert.h"
/// @par
///
/// Basically, any spans that are closer to a boundary or obstruction than the specified radius
/// are marked as unwalkable.
///
/// This method is usually called immediately after the heightfield has been built.
///
/// @see rcCompactHeightfield, rcBuildCompactHeightfield, rcConfig::walkableRadius
bool rcErodeWalkableArea(rcContext* ctx, int radius, rcCompactHeightfield& chf)
{
rcAssert(ctx);
const int w = chf.width;
const int h = chf.height;
rcScopedTimer timer(ctx, RC_TIMER_ERODE_AREA);
unsigned char* dist = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
if (!dist)
{
ctx->log(RC_LOG_ERROR, "erodeWalkableArea: Out of memory 'dist' (%d).", chf.spanCount);
return false;
}
// Init distance.
memset(dist, 0xff, sizeof(unsigned char)*chf.spanCount);
// Mark boundary cells.
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
if (chf.areas[i] == RC_NULL_AREA)
{
dist[i] = 0;
}
else
{
const rcCompactSpan& s = chf.spans[i];
int nc = 0;
for (int dir = 0; dir < 4; ++dir)
{
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const int nx = x + rcGetDirOffsetX(dir);
const int ny = y + rcGetDirOffsetY(dir);
const int nidx = (int)chf.cells[nx+ny*w].index + rcGetCon(s, dir);
if (chf.areas[nidx] != RC_NULL_AREA)
{
nc++;
}
}
}
// At least one missing neighbour.
if (nc != 4)
dist[i] = 0;
}
}
}
}
unsigned char nd;
// Pass 1
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
const rcCompactSpan& s = chf.spans[i];
if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
{
// (-1,0)
const int ax = x + rcGetDirOffsetX(0);
const int ay = y + rcGetDirOffsetY(0);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
const rcCompactSpan& as = chf.spans[ai];
nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
if (nd < dist[i])
dist[i] = nd;
// (-1,-1)
if (rcGetCon(as, 3) != RC_NOT_CONNECTED)
{
const int aax = ax + rcGetDirOffsetX(3);
const int aay = ay + rcGetDirOffsetY(3);
const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 3);
nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
if (nd < dist[i])
dist[i] = nd;
}
}
if (rcGetCon(s, 3) != RC_NOT_CONNECTED)
{
// (0,-1)
const int ax = x + rcGetDirOffsetX(3);
const int ay = y + rcGetDirOffsetY(3);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
const rcCompactSpan& as = chf.spans[ai];
nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
if (nd < dist[i])
dist[i] = nd;
// (1,-1)
if (rcGetCon(as, 2) != RC_NOT_CONNECTED)
{
const int aax = ax + rcGetDirOffsetX(2);
const int aay = ay + rcGetDirOffsetY(2);
const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 2);
nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
if (nd < dist[i])
dist[i] = nd;
}
}
}
}
}
// Pass 2
for (int y = h-1; y >= 0; --y)
{
for (int x = w-1; x >= 0; --x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
const rcCompactSpan& s = chf.spans[i];
if (rcGetCon(s, 2) != RC_NOT_CONNECTED)
{
// (1,0)
const int ax = x + rcGetDirOffsetX(2);
const int ay = y + rcGetDirOffsetY(2);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 2);
const rcCompactSpan& as = chf.spans[ai];
nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
if (nd < dist[i])
dist[i] = nd;
// (1,1)
if (rcGetCon(as, 1) != RC_NOT_CONNECTED)
{
const int aax = ax + rcGetDirOffsetX(1);
const int aay = ay + rcGetDirOffsetY(1);
const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 1);
nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
if (nd < dist[i])
dist[i] = nd;
}
}
if (rcGetCon(s, 1) != RC_NOT_CONNECTED)
{
// (0,1)
const int ax = x + rcGetDirOffsetX(1);
const int ay = y + rcGetDirOffsetY(1);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 1);
const rcCompactSpan& as = chf.spans[ai];
nd = (unsigned char)rcMin((int)dist[ai]+2, 255);
if (nd < dist[i])
dist[i] = nd;
// (-1,1)
if (rcGetCon(as, 0) != RC_NOT_CONNECTED)
{
const int aax = ax + rcGetDirOffsetX(0);
const int aay = ay + rcGetDirOffsetY(0);
const int aai = (int)chf.cells[aax+aay*w].index + rcGetCon(as, 0);
nd = (unsigned char)rcMin((int)dist[aai]+3, 255);
if (nd < dist[i])
dist[i] = nd;
}
}
}
}
}
const unsigned char thr = (unsigned char)(radius*2);
for (int i = 0; i < chf.spanCount; ++i)
if (dist[i] < thr)
chf.areas[i] = RC_NULL_AREA;
rcFree(dist);
return true;
}
static void insertSort(unsigned char* a, const int n)
{
int i, j;
for (i = 1; i < n; i++)
{
const unsigned char value = a[i];
for (j = i - 1; j >= 0 && a[j] > value; j--)
a[j+1] = a[j];
a[j+1] = value;
}
}
/// @par
///
/// This filter is usually applied after applying area id's using functions
/// such as #rcMarkBoxArea, #rcMarkConvexPolyArea, and #rcMarkCylinderArea.
///
/// @see rcCompactHeightfield
bool rcMedianFilterWalkableArea(rcContext* ctx, rcCompactHeightfield& chf)
{
rcAssert(ctx);
const int w = chf.width;
const int h = chf.height;
rcScopedTimer timer(ctx, RC_TIMER_MEDIAN_AREA);
unsigned char* areas = (unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP);
if (!areas)
{
ctx->log(RC_LOG_ERROR, "medianFilterWalkableArea: Out of memory 'areas' (%d).", chf.spanCount);
return false;
}
// Init distance.
memset(areas, 0xff, sizeof(unsigned char)*chf.spanCount);
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
const rcCompactSpan& s = chf.spans[i];
if (chf.areas[i] == RC_NULL_AREA)
{
areas[i] = chf.areas[i];
continue;
}
unsigned char nei[9];
for (int j = 0; j < 9; ++j)
nei[j] = chf.areas[i];
for (int dir = 0; dir < 4; ++dir)
{
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const int ax = x + rcGetDirOffsetX(dir);
const int ay = y + rcGetDirOffsetY(dir);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
if (chf.areas[ai] != RC_NULL_AREA)
nei[dir*2+0] = chf.areas[ai];
const rcCompactSpan& as = chf.spans[ai];
const int dir2 = (dir+1) & 0x3;
if (rcGetCon(as, dir2) != RC_NOT_CONNECTED)
{
const int ax2 = ax + rcGetDirOffsetX(dir2);
const int ay2 = ay + rcGetDirOffsetY(dir2);
const int ai2 = (int)chf.cells[ax2+ay2*w].index + rcGetCon(as, dir2);
if (chf.areas[ai2] != RC_NULL_AREA)
nei[dir*2+1] = chf.areas[ai2];
}
}
}
insertSort(nei, 9);
areas[i] = nei[4];
}
}
}
memcpy(chf.areas, areas, sizeof(unsigned char)*chf.spanCount);
rcFree(areas);
return true;
}
/// @par
///
/// The value of spacial parameters are in world units.
///
/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
void rcMarkBoxArea(rcContext* ctx, const float* bmin, const float* bmax, unsigned char areaId,
rcCompactHeightfield& chf)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_MARK_BOX_AREA);
int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
if (maxx < 0) return;
if (minx >= chf.width) return;
if (maxz < 0) return;
if (minz >= chf.height) return;
if (minx < 0) minx = 0;
if (maxx >= chf.width) maxx = chf.width-1;
if (minz < 0) minz = 0;
if (maxz >= chf.height) maxz = chf.height-1;
for (int z = minz; z <= maxz; ++z)
{
for (int x = minx; x <= maxx; ++x)
{
const rcCompactCell& c = chf.cells[x+z*chf.width];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
rcCompactSpan& s = chf.spans[i];
if ((int)s.y >= miny && (int)s.y <= maxy)
{
if (chf.areas[i] != RC_NULL_AREA)
chf.areas[i] = areaId;
}
}
}
}
}
static int pointInPoly(int nvert, const float* verts, const float* p)
{
int i, j, c = 0;
for (i = 0, j = nvert-1; i < nvert; j = i++)
{
const float* vi = &verts[i*3];
const float* vj = &verts[j*3];
if (((vi[2] > p[2]) != (vj[2] > p[2])) &&
(p[0] < (vj[0]-vi[0]) * (p[2]-vi[2]) / (vj[2]-vi[2]) + vi[0]) )
c = !c;
}
return c;
}
/// @par
///
/// The value of spacial parameters are in world units.
///
/// The y-values of the polygon vertices are ignored. So the polygon is effectively
/// projected onto the xz-plane at @p hmin, then extruded to @p hmax.
///
/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
void rcMarkConvexPolyArea(rcContext* ctx, const float* verts, const int nverts,
const float hmin, const float hmax, unsigned char areaId,
rcCompactHeightfield& chf)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_MARK_CONVEXPOLY_AREA);
float bmin[3], bmax[3];
rcVcopy(bmin, verts);
rcVcopy(bmax, verts);
for (int i = 1; i < nverts; ++i)
{
rcVmin(bmin, &verts[i*3]);
rcVmax(bmax, &verts[i*3]);
}
bmin[1] = hmin;
bmax[1] = hmax;
int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
if (maxx < 0) return;
if (minx >= chf.width) return;
if (maxz < 0) return;
if (minz >= chf.height) return;
if (minx < 0) minx = 0;
if (maxx >= chf.width) maxx = chf.width-1;
if (minz < 0) minz = 0;
if (maxz >= chf.height) maxz = chf.height-1;
// TODO: Optimize.
for (int z = minz; z <= maxz; ++z)
{
for (int x = minx; x <= maxx; ++x)
{
const rcCompactCell& c = chf.cells[x+z*chf.width];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
rcCompactSpan& s = chf.spans[i];
if (chf.areas[i] == RC_NULL_AREA)
continue;
if ((int)s.y >= miny && (int)s.y <= maxy)
{
float p[3];
p[0] = chf.bmin[0] + (x+0.5f)*chf.cs;
p[1] = 0;
p[2] = chf.bmin[2] + (z+0.5f)*chf.cs;
if (pointInPoly(nverts, verts, p))
{
chf.areas[i] = areaId;
}
}
}
}
}
}
int rcOffsetPoly(const float* verts, const int nverts, const float offset,
float* outVerts, const int maxOutVerts)
{
const float MITER_LIMIT = 1.20f;
int n = 0;
for (int i = 0; i < nverts; i++)
{
const int a = (i+nverts-1) % nverts;
const int b = i;
const int c = (i+1) % nverts;
const float* va = &verts[a*3];
const float* vb = &verts[b*3];
const float* vc = &verts[c*3];
float dx0 = vb[0] - va[0];
float dy0 = vb[2] - va[2];
float d0 = dx0*dx0 + dy0*dy0;
if (d0 > 1e-6f)
{
d0 = 1.0f/rcSqrt(d0);
dx0 *= d0;
dy0 *= d0;
}
float dx1 = vc[0] - vb[0];
float dy1 = vc[2] - vb[2];
float d1 = dx1*dx1 + dy1*dy1;
if (d1 > 1e-6f)
{
d1 = 1.0f/rcSqrt(d1);
dx1 *= d1;
dy1 *= d1;
}
const float dlx0 = -dy0;
const float dly0 = dx0;
const float dlx1 = -dy1;
const float dly1 = dx1;
float cross = dx1*dy0 - dx0*dy1;
float dmx = (dlx0 + dlx1) * 0.5f;
float dmy = (dly0 + dly1) * 0.5f;
float dmr2 = dmx*dmx + dmy*dmy;
bool bevel = dmr2 * MITER_LIMIT*MITER_LIMIT < 1.0f;
if (dmr2 > 1e-6f)
{
const float scale = 1.0f / dmr2;
dmx *= scale;
dmy *= scale;
}
if (bevel && cross < 0.0f)
{
if (n+2 >= maxOutVerts)
return 0;
float d = (1.0f - (dx0*dx1 + dy0*dy1))*0.5f;
outVerts[n*3+0] = vb[0] + (-dlx0+dx0*d)*offset;
outVerts[n*3+1] = vb[1];
outVerts[n*3+2] = vb[2] + (-dly0+dy0*d)*offset;
n++;
outVerts[n*3+0] = vb[0] + (-dlx1-dx1*d)*offset;
outVerts[n*3+1] = vb[1];
outVerts[n*3+2] = vb[2] + (-dly1-dy1*d)*offset;
n++;
}
else
{
if (n+1 >= maxOutVerts)
return 0;
outVerts[n*3+0] = vb[0] - dmx*offset;
outVerts[n*3+1] = vb[1];
outVerts[n*3+2] = vb[2] - dmy*offset;
n++;
}
}
return n;
}
/// @par
///
/// The value of spacial parameters are in world units.
///
/// @see rcCompactHeightfield, rcMedianFilterWalkableArea
void rcMarkCylinderArea(rcContext* ctx, const float* pos,
const float r, const float h, unsigned char areaId,
rcCompactHeightfield& chf)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_MARK_CYLINDER_AREA);
float bmin[3], bmax[3];
bmin[0] = pos[0] - r;
bmin[1] = pos[1];
bmin[2] = pos[2] - r;
bmax[0] = pos[0] + r;
bmax[1] = pos[1] + h;
bmax[2] = pos[2] + r;
const float r2 = r*r;
int minx = (int)((bmin[0]-chf.bmin[0])/chf.cs);
int miny = (int)((bmin[1]-chf.bmin[1])/chf.ch);
int minz = (int)((bmin[2]-chf.bmin[2])/chf.cs);
int maxx = (int)((bmax[0]-chf.bmin[0])/chf.cs);
int maxy = (int)((bmax[1]-chf.bmin[1])/chf.ch);
int maxz = (int)((bmax[2]-chf.bmin[2])/chf.cs);
if (maxx < 0) return;
if (minx >= chf.width) return;
if (maxz < 0) return;
if (minz >= chf.height) return;
if (minx < 0) minx = 0;
if (maxx >= chf.width) maxx = chf.width-1;
if (minz < 0) minz = 0;
if (maxz >= chf.height) maxz = chf.height-1;
for (int z = minz; z <= maxz; ++z)
{
for (int x = minx; x <= maxx; ++x)
{
const rcCompactCell& c = chf.cells[x+z*chf.width];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
rcCompactSpan& s = chf.spans[i];
if (chf.areas[i] == RC_NULL_AREA)
continue;
if ((int)s.y >= miny && (int)s.y <= maxy)
{
const float sx = chf.bmin[0] + (x+0.5f)*chf.cs;
const float sz = chf.bmin[2] + (z+0.5f)*chf.cs;
const float dx = sx - pos[0];
const float dz = sz - pos[2];
if (dx*dx + dz*dz < r2)
{
chf.areas[i] = areaId;
}
}
}
}
}
}

35
thirdparty/recastnavigation/Recast/Source/RecastAssert.cpp vendored
View File

@ -1,35 +0,0 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include "RecastAssert.h"
#ifndef NDEBUG
static rcAssertFailFunc* sRecastAssertFailFunc = 0;
void rcAssertFailSetCustom(rcAssertFailFunc* assertFailFunc)
{
sRecastAssertFailFunc = assertFailFunc;
}
rcAssertFailFunc* rcAssertFailGetCustom()
{
return sRecastAssertFailFunc;
}
#endif

1104
thirdparty/recastnavigation/Recast/Source/RecastContour.cpp vendored
View File

File diff suppressed because it is too large Load Diff

184
thirdparty/recastnavigation/Recast/Source/RecastFilter.cpp vendored
View File

@ -1,184 +0,0 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include "Recast.h"
#include "RecastAssert.h"
#include <stdlib.h>
void rcFilterLowHangingWalkableObstacles(rcContext* context, const int walkableClimb, rcHeightfield& heightfield)
{
rcAssert(context);
rcScopedTimer timer(context, RC_TIMER_FILTER_LOW_OBSTACLES);
const int xSize = heightfield.width;
const int zSize = heightfield.height;
for (int z = 0; z < zSize; ++z)
{
for (int x = 0; x < xSize; ++x)
{
rcSpan* previousSpan = NULL;
bool previousWasWalkable = false;
unsigned char previousArea = RC_NULL_AREA;
for (rcSpan* span = heightfield.spans[x + z * xSize]; span != NULL; previousSpan = span, span = span->next)
{
const bool walkable = span->area != RC_NULL_AREA;
// If current span is not walkable, but there is walkable
// span just below it, mark the span above it walkable too.
if (!walkable && previousWasWalkable)
{
if (rcAbs((int)span->smax - (int)previousSpan->smax) <= walkableClimb)
{
span->area = previousArea;
}
}
// Copy walkable flag so that it cannot propagate
// past multiple non-walkable objects.
previousWasWalkable = walkable;
previousArea = span->area;
}
}
}
}
void rcFilterLedgeSpans(rcContext* context, const int walkableHeight, const int walkableClimb,
rcHeightfield& heightfield)
{
rcAssert(context);
rcScopedTimer timer(context, RC_TIMER_FILTER_BORDER);
const int xSize = heightfield.width;
const int zSize = heightfield.height;
const int MAX_HEIGHT = 0xffff; // TODO (graham): Move this to a more visible constant and update usages.
// Mark border spans.
for (int z = 0; z < zSize; ++z)
{
for (int x = 0; x < xSize; ++x)
{
for (rcSpan* span = heightfield.spans[x + z * xSize]; span; span = span->next)
{
// Skip non walkable spans.
if (span->area == RC_NULL_AREA)
{
continue;
}
const int bot = (int)(span->smax);
const int top = span->next ? (int)(span->next->smin) : MAX_HEIGHT;
// Find neighbours minimum height.
int minNeighborHeight = MAX_HEIGHT;
// Min and max height of accessible neighbours.
int accessibleNeighborMinHeight = span->smax;
int accessibleNeighborMaxHeight = span->smax;
for (int direction = 0; direction < 4; ++direction)
{
int dx = x + rcGetDirOffsetX(direction);
int dy = z + rcGetDirOffsetY(direction);
// Skip neighbours which are out of bounds.
if (dx < 0 || dy < 0 || dx >= xSize || dy >= zSize)
{
minNeighborHeight = rcMin(minNeighborHeight, -walkableClimb - bot);
continue;
}
// From minus infinity to the first span.
const rcSpan* neighborSpan = heightfield.spans[dx + dy * xSize];
int neighborBot = -walkableClimb;
int neighborTop = neighborSpan ? (int)neighborSpan->smin : MAX_HEIGHT;
// Skip neighbour if the gap between the spans is too small.
if (rcMin(top, neighborTop) - rcMax(bot, neighborBot) > walkableHeight)
{
minNeighborHeight = rcMin(minNeighborHeight, neighborBot - bot);
}
// Rest of the spans.
for (neighborSpan = heightfield.spans[dx + dy * xSize]; neighborSpan; neighborSpan = neighborSpan->next)
{
neighborBot = (int)neighborSpan->smax;
neighborTop = neighborSpan->next ? (int)neighborSpan->next->smin : MAX_HEIGHT;
// Skip neighbour if the gap between the spans is too small.
if (rcMin(top, neighborTop) - rcMax(bot, neighborBot) > walkableHeight)
{
minNeighborHeight = rcMin(minNeighborHeight, neighborBot - bot);
// Find min/max accessible neighbour height.
if (rcAbs(neighborBot - bot) <= walkableClimb)
{
if (neighborBot < accessibleNeighborMinHeight) accessibleNeighborMinHeight = neighborBot;
if (neighborBot > accessibleNeighborMaxHeight) accessibleNeighborMaxHeight = neighborBot;
}
}
}
}
// The current span is close to a ledge if the drop to any
// neighbour span is less than the walkableClimb.
if (minNeighborHeight < -walkableClimb)
{
span->area = RC_NULL_AREA;
}
// If the difference between all neighbours is too large,
// we are at steep slope, mark the span as ledge.
else if ((accessibleNeighborMaxHeight - accessibleNeighborMinHeight) > walkableClimb)
{
span->area = RC_NULL_AREA;
}
}
}
}
}
void rcFilterWalkableLowHeightSpans(rcContext* context, const int walkableHeight, rcHeightfield& heightfield)
{
rcAssert(context);
rcScopedTimer timer(context, RC_TIMER_FILTER_WALKABLE);
const int xSize = heightfield.width;
const int zSize = heightfield.height;
const int MAX_HEIGHT = 0xffff;
// Remove walkable flag from spans which do not have enough
// space above them for the agent to stand there.
for (int z = 0; z < zSize; ++z)
{
for (int x = 0; x < xSize; ++x)
{
for (rcSpan* span = heightfield.spans[x + z*xSize]; span; span = span->next)
{
const int bot = (int)(span->smax);
const int top = span->next ? (int)(span->next->smin) : MAX_HEIGHT;
if ((top - bot) < walkableHeight)
{
span->area = RC_NULL_AREA;
}
}
}
}
}

656
thirdparty/recastnavigation/Recast/Source/RecastLayers.cpp vendored
View File

@ -1,656 +0,0 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include <float.h>
#include <math.h>
#include <string.h>
#include <stdlib.h>
#include <stdio.h>
#include "Recast.h"
#include "RecastAlloc.h"
#include "RecastAssert.h"
// Must be 255 or smaller (not 256) because layer IDs are stored as
// a byte where 255 is a special value.
#ifndef RC_MAX_LAYERS_DEF
#define RC_MAX_LAYERS_DEF 63
#endif
#if RC_MAX_LAYERS_DEF > 255
#error RC_MAX_LAYERS_DEF must be 255 or smaller
#endif
#ifndef RC_MAX_NEIS_DEF
#define RC_MAX_NEIS_DEF 16
#endif
// Keep type checking.
static const int RC_MAX_LAYERS = RC_MAX_LAYERS_DEF;
static const int RC_MAX_NEIS = RC_MAX_NEIS_DEF;
struct rcLayerRegion
{
unsigned char layers[RC_MAX_LAYERS];
unsigned char neis[RC_MAX_NEIS];
unsigned short ymin, ymax;
unsigned char layerId; // Layer ID
unsigned char nlayers; // Layer count
unsigned char nneis; // Neighbour count
unsigned char base; // Flag indicating if the region is the base of merged regions.
};
static bool contains(const unsigned char* a, const unsigned char an, const unsigned char v)
{
const int n = (int)an;
for (int i = 0; i < n; ++i)
{
if (a[i] == v)
return true;
}
return false;
}
static bool addUnique(unsigned char* a, unsigned char& an, int anMax, unsigned char v)
{
if (contains(a, an, v))
return true;
if ((int)an >= anMax)
return false;
a[an] = v;
an++;
return true;
}
inline bool overlapRange(const unsigned short amin, const unsigned short amax,
const unsigned short bmin, const unsigned short bmax)
{
return (amin > bmax || amax < bmin) ? false : true;
}
struct rcLayerSweepSpan
{
unsigned short ns; // number samples
unsigned char id; // region id
unsigned char nei; // neighbour id
};
/// @par
///
/// See the #rcConfig documentation for more information on the configuration parameters.
///
/// @see rcAllocHeightfieldLayerSet, rcCompactHeightfield, rcHeightfieldLayerSet, rcConfig
bool rcBuildHeightfieldLayers(rcContext* ctx, const rcCompactHeightfield& chf,
const int borderSize, const int walkableHeight,
rcHeightfieldLayerSet& lset)
{
rcAssert(ctx);
rcScopedTimer timer(ctx, RC_TIMER_BUILD_LAYERS);
const int w = chf.width;
const int h = chf.height;
rcScopedDelete<unsigned char> srcReg((unsigned char*)rcAlloc(sizeof(unsigned char)*chf.spanCount, RC_ALLOC_TEMP));
if (!srcReg)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'srcReg' (%d).", chf.spanCount);
return false;
}
memset(srcReg,0xff,sizeof(unsigned char)*chf.spanCount);
const int nsweeps = chf.width;
rcScopedDelete<rcLayerSweepSpan> sweeps((rcLayerSweepSpan*)rcAlloc(sizeof(rcLayerSweepSpan)*nsweeps, RC_ALLOC_TEMP));
if (!sweeps)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'sweeps' (%d).", nsweeps);
return false;
}
// Partition walkable area into monotone regions.
int prevCount[256];
unsigned char regId = 0;
for (int y = borderSize; y < h-borderSize; ++y)
{
memset(prevCount,0,sizeof(int)*regId);
unsigned char sweepId = 0;
for (int x = borderSize; x < w-borderSize; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
const rcCompactSpan& s = chf.spans[i];
if (chf.areas[i] == RC_NULL_AREA) continue;
unsigned char sid = 0xff;
// -x
if (rcGetCon(s, 0) != RC_NOT_CONNECTED)
{
const int ax = x + rcGetDirOffsetX(0);
const int ay = y + rcGetDirOffsetY(0);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 0);
if (chf.areas[ai] != RC_NULL_AREA && srcReg[ai] != 0xff)
sid = srcReg[ai];
}
if (sid == 0xff)
{
sid = sweepId++;
sweeps[sid].nei = 0xff;
sweeps[sid].ns = 0;
}
// -y
if (rcGetCon(s,3) != RC_NOT_CONNECTED)
{
const int ax = x + rcGetDirOffsetX(3);
const int ay = y + rcGetDirOffsetY(3);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, 3);
const unsigned char nr = srcReg[ai];
if (nr != 0xff)
{
// Set neighbour when first valid neighbour is encoutered.
if (sweeps[sid].ns == 0)
sweeps[sid].nei = nr;
if (sweeps[sid].nei == nr)
{
// Update existing neighbour
sweeps[sid].ns++;
prevCount[nr]++;
}
else
{
// This is hit if there is nore than one neighbour.
// Invalidate the neighbour.
sweeps[sid].nei = 0xff;
}
}
}
srcReg[i] = sid;
}
}
// Create unique ID.
for (int i = 0; i < sweepId; ++i)
{
// If the neighbour is set and there is only one continuous connection to it,
// the sweep will be merged with the previous one, else new region is created.
if (sweeps[i].nei != 0xff && prevCount[sweeps[i].nei] == (int)sweeps[i].ns)
{
sweeps[i].id = sweeps[i].nei;
}
else
{
if (regId == 255)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Region ID overflow.");
return false;
}
sweeps[i].id = regId++;
}
}
// Remap local sweep ids to region ids.
for (int x = borderSize; x < w-borderSize; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
if (srcReg[i] != 0xff)
srcReg[i] = sweeps[srcReg[i]].id;
}
}
}
// Allocate and init layer regions.
const int nregs = (int)regId;
rcScopedDelete<rcLayerRegion> regs((rcLayerRegion*)rcAlloc(sizeof(rcLayerRegion)*nregs, RC_ALLOC_TEMP));
if (!regs)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'regs' (%d).", nregs);
return false;
}
memset(regs, 0, sizeof(rcLayerRegion)*nregs);
for (int i = 0; i < nregs; ++i)
{
regs[i].layerId = 0xff;
regs[i].ymin = 0xffff;
regs[i].ymax = 0;
}
// Find region neighbours and overlapping regions.
for (int y = 0; y < h; ++y)
{
for (int x = 0; x < w; ++x)
{
const rcCompactCell& c = chf.cells[x+y*w];
unsigned char lregs[RC_MAX_LAYERS];
int nlregs = 0;
for (int i = (int)c.index, ni = (int)(c.index+c.count); i < ni; ++i)
{
const rcCompactSpan& s = chf.spans[i];
const unsigned char ri = srcReg[i];
if (ri == 0xff) continue;
regs[ri].ymin = rcMin(regs[ri].ymin, s.y);
regs[ri].ymax = rcMax(regs[ri].ymax, s.y);
// Collect all region layers.
if (nlregs < RC_MAX_LAYERS)
lregs[nlregs++] = ri;
// Update neighbours
for (int dir = 0; dir < 4; ++dir)
{
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const int ax = x + rcGetDirOffsetX(dir);
const int ay = y + rcGetDirOffsetY(dir);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
const unsigned char rai = srcReg[ai];
if (rai != 0xff && rai != ri)
{
// Don't check return value -- if we cannot add the neighbor
// it will just cause a few more regions to be created, which
// is fine.
addUnique(regs[ri].neis, regs[ri].nneis, RC_MAX_NEIS, rai);
}
}
}
}
// Update overlapping regions.
for (int i = 0; i < nlregs-1; ++i)
{
for (int j = i+1; j < nlregs; ++j)
{
if (lregs[i] != lregs[j])
{
rcLayerRegion& ri = regs[lregs[i]];
rcLayerRegion& rj = regs[lregs[j]];
if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, lregs[j]) ||
!addUnique(rj.layers, rj.nlayers, RC_MAX_LAYERS, lregs[i]))
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
return false;
}
}
}
}
}
}
// Create 2D layers from regions.
unsigned char layerId = 0;
static const int MAX_STACK = 64;
unsigned char stack[MAX_STACK];
int nstack = 0;
for (int i = 0; i < nregs; ++i)
{
rcLayerRegion& root = regs[i];
// Skip already visited.
if (root.layerId != 0xff)
continue;
// Start search.
root.layerId = layerId;
root.base = 1;
nstack = 0;
stack[nstack++] = (unsigned char)i;
while (nstack)
{
// Pop front
rcLayerRegion& reg = regs[stack[0]];
nstack--;
for (int j = 0; j < nstack; ++j)
stack[j] = stack[j+1];
const int nneis = (int)reg.nneis;
for (int j = 0; j < nneis; ++j)
{
const unsigned char nei = reg.neis[j];
rcLayerRegion& regn = regs[nei];
// Skip already visited.
if (regn.layerId != 0xff)
continue;
// Skip if the neighbour is overlapping root region.
if (contains(root.layers, root.nlayers, nei))
continue;
// Skip if the height range would become too large.
const int ymin = rcMin(root.ymin, regn.ymin);
const int ymax = rcMax(root.ymax, regn.ymax);
if ((ymax - ymin) >= 255)
continue;
if (nstack < MAX_STACK)
{
// Deepen
stack[nstack++] = (unsigned char)nei;
// Mark layer id
regn.layerId = layerId;
// Merge current layers to root.
for (int k = 0; k < regn.nlayers; ++k)
{
if (!addUnique(root.layers, root.nlayers, RC_MAX_LAYERS, regn.layers[k]))
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
return false;
}
}
root.ymin = rcMin(root.ymin, regn.ymin);
root.ymax = rcMax(root.ymax, regn.ymax);
}
}
}
layerId++;
}
// Merge non-overlapping regions that are close in height.
const unsigned short mergeHeight = (unsigned short)walkableHeight * 4;
for (int i = 0; i < nregs; ++i)
{
rcLayerRegion& ri = regs[i];
if (!ri.base) continue;
unsigned char newId = ri.layerId;
for (;;)
{
unsigned char oldId = 0xff;
for (int j = 0; j < nregs; ++j)
{
if (i == j) continue;
rcLayerRegion& rj = regs[j];
if (!rj.base) continue;
// Skip if the regions are not close to each other.
if (!overlapRange(ri.ymin,ri.ymax+mergeHeight, rj.ymin,rj.ymax+mergeHeight))
continue;
// Skip if the height range would become too large.
const int ymin = rcMin(ri.ymin, rj.ymin);
const int ymax = rcMax(ri.ymax, rj.ymax);
if ((ymax - ymin) >= 255)
continue;
// Make sure that there is no overlap when merging 'ri' and 'rj'.
bool overlap = false;
// Iterate over all regions which have the same layerId as 'rj'
for (int k = 0; k < nregs; ++k)
{
if (regs[k].layerId != rj.layerId)
continue;
// Check if region 'k' is overlapping region 'ri'
// Index to 'regs' is the same as region id.
if (contains(ri.layers,ri.nlayers, (unsigned char)k))
{
overlap = true;
break;
}
}
// Cannot merge of regions overlap.
if (overlap)
continue;
// Can merge i and j.
oldId = rj.layerId;
break;
}
// Could not find anything to merge with, stop.
if (oldId == 0xff)
break;
// Merge
for (int j = 0; j < nregs; ++j)
{
rcLayerRegion& rj = regs[j];
if (rj.layerId == oldId)
{
rj.base = 0;
// Remap layerIds.
rj.layerId = newId;
// Add overlaid layers from 'rj' to 'ri'.
for (int k = 0; k < rj.nlayers; ++k)
{
if (!addUnique(ri.layers, ri.nlayers, RC_MAX_LAYERS, rj.layers[k]))
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: layer overflow (too many overlapping walkable platforms). Try increasing RC_MAX_LAYERS.");
return false;
}
}
// Update height bounds.
ri.ymin = rcMin(ri.ymin, rj.ymin);
ri.ymax = rcMax(ri.ymax, rj.ymax);
}
}
}
}
// Compact layerIds
unsigned char remap[256];
memset(remap, 0, 256);
// Find number of unique layers.
layerId = 0;
for (int i = 0; i < nregs; ++i)
remap[regs[i].layerId] = 1;
for (int i = 0; i < 256; ++i)
{
if (remap[i])
remap[i] = layerId++;
else
remap[i] = 0xff;
}
// Remap ids.
for (int i = 0; i < nregs; ++i)
regs[i].layerId = remap[regs[i].layerId];
// No layers, return empty.
if (layerId == 0)
return true;
// Create layers.
rcAssert(lset.layers == 0);
const int lw = w - borderSize*2;
const int lh = h - borderSize*2;
// Build contracted bbox for layers.
float bmin[3], bmax[3];
rcVcopy(bmin, chf.bmin);
rcVcopy(bmax, chf.bmax);
bmin[0] += borderSize*chf.cs;
bmin[2] += borderSize*chf.cs;
bmax[0] -= borderSize*chf.cs;
bmax[2] -= borderSize*chf.cs;
lset.nlayers = (int)layerId;
lset.layers = (rcHeightfieldLayer*)rcAlloc(sizeof(rcHeightfieldLayer)*lset.nlayers, RC_ALLOC_PERM);
if (!lset.layers)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'layers' (%d).", lset.nlayers);
return false;
}
memset(lset.layers, 0, sizeof(rcHeightfieldLayer)*lset.nlayers);
// Store layers.
for (int i = 0; i < lset.nlayers; ++i)
{
unsigned char curId = (unsigned char)i;
rcHeightfieldLayer* layer = &lset.layers[i];
const int gridSize = sizeof(unsigned char)*lw*lh;
layer->heights = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
if (!layer->heights)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'heights' (%d).", gridSize);
return false;
}
memset(layer->heights, 0xff, gridSize);
layer->areas = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
if (!layer->areas)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'areas' (%d).", gridSize);
return false;
}
memset(layer->areas, 0, gridSize);
layer->cons = (unsigned char*)rcAlloc(gridSize, RC_ALLOC_PERM);
if (!layer->cons)
{
ctx->log(RC_LOG_ERROR, "rcBuildHeightfieldLayers: Out of memory 'cons' (%d).", gridSize);
return false;
}
memset(layer->cons, 0, gridSize);
// Find layer height bounds.
int hmin = 0, hmax = 0;
for (int j = 0; j < nregs; ++j)
{
if (regs[j].base && regs[j].layerId == curId)
{
hmin = (int)regs[j].ymin;
hmax = (int)regs[j].ymax;
}
}
layer->width = lw;
layer->height = lh;
layer->cs = chf.cs;
layer->ch = chf.ch;
// Adjust the bbox to fit the heightfield.
rcVcopy(layer->bmin, bmin);
rcVcopy(layer->bmax, bmax);
layer->bmin[1] = bmin[1] + hmin*chf.ch;
layer->bmax[1] = bmin[1] + hmax*chf.ch;
layer->hmin = hmin;
layer->hmax = hmax;
// Update usable data region.
layer->minx = layer->width;
layer->maxx = 0;
layer->miny = layer->height;
layer->maxy = 0;
// Copy height and area from compact heightfield.
for (int y = 0; y < lh; ++y)
{
for (int x = 0; x < lw; ++x)
{
const int cx = borderSize+x;
const int cy = borderSize+y;
const rcCompactCell& c = chf.cells[cx+cy*w];
for (int j = (int)c.index, nj = (int)(c.index+c.count); j < nj; ++j)
{
const rcCompactSpan& s = chf.spans[j];
// Skip unassigned regions.
if (srcReg[j] == 0xff)
continue;
// Skip of does nto belong to current layer.
unsigned char lid = regs[srcReg[j]].layerId;
if (lid != curId)
continue;
// Update data bounds.
layer->minx = rcMin(layer->minx, x);
layer->maxx = rcMax(layer->maxx, x);
layer->miny = rcMin(layer->miny, y);
layer->maxy = rcMax(layer->maxy, y);
// Store height and area type.
const int idx = x+y*lw;
layer->heights[idx] = (unsigned char)(s.y - hmin);
layer->areas[idx] = chf.areas[j];
// Check connection.
unsigned char portal = 0;
unsigned char con = 0;
for (int dir = 0; dir < 4; ++dir)
{
if (rcGetCon(s, dir) != RC_NOT_CONNECTED)
{
const int ax = cx + rcGetDirOffsetX(dir);
const int ay = cy + rcGetDirOffsetY(dir);
const int ai = (int)chf.cells[ax+ay*w].index + rcGetCon(s, dir);
unsigned char alid = srcReg[ai] != 0xff ? regs[srcReg[ai]].layerId : 0xff;
// Portal mask
if (chf.areas[ai] != RC_NULL_AREA && lid != alid)
{
portal |= (unsigned char)(1<<dir);
// Update height so that it matches on both sides of the portal.
const rcCompactSpan& as = chf.spans[ai];
if (as.y > hmin)
layer->heights[idx] = rcMax(layer->heights[idx], (unsigned char)(as.y - hmin));
}
// Valid connection mask
if (chf.areas[ai] != RC_NULL_AREA && lid == alid)
{
const int nx = ax - borderSize;
const int ny = ay - borderSize;
if (nx >= 0 && ny >= 0 && nx < lw && ny < lh)
con |= (unsigned char)(1<<dir);
}
}
}
layer->cons[idx] = (portal << 4) | con;
}
}
}
if (layer->minx > layer->maxx)
layer->minx = layer->maxx = 0;
if (layer->miny > layer->maxy)
layer->miny = layer->maxy = 0;
}
return true;
}

1549
thirdparty/recastnavigation/Recast/Source/RecastMesh.cpp vendored
View File

File diff suppressed because it is too large Load Diff

1463
thirdparty/recastnavigation/Recast/Source/RecastMeshDetail.cpp vendored
View File

File diff suppressed because it is too large Load Diff

556
thirdparty/recastnavigation/Recast/Source/RecastRasterization.cpp vendored
View File

@ -1,556 +0,0 @@
//
// Copyright (c) 2009-2010 Mikko Mononen memon@inside.org
//
// This software is provided 'as-is', without any express or implied
// warranty. In no event will the authors be held liable for any damages
// arising from the use of this software.
// Permission is granted to anyone to use this software for any purpose,
// including commercial applications, and to alter it and redistribute it
// freely, subject to the following restrictions:
// 1. The origin of this software must not be misrepresented; you must not
// claim that you wrote the original software. If you use this software
// in a product, an acknowledgment in the product documentation would be
// appreciated but is not required.
// 2. Altered source versions must be plainly marked as such, and must not be
// misrepresented as being the original software.
// 3. This notice may not be removed or altered from any source distribution.
//
#include <math.h>
#include <stdio.h>
#include "Recast.h"
#include "RecastAlloc.h"
#include "RecastAssert.h"
/// Check whether two bounding boxes overlap
///
/// @param[in] aMin Min axis extents of bounding box A
/// @param[in] aMax Max axis extents of bounding box A
/// @param[in] bMin Min axis extents of bounding box B
/// @param[in] bMax Max axis extents of bounding box B
/// @returns true if the two bounding boxes overlap. False otherwise.
static bool overlapBounds(const float* aMin, const float* aMax, const float* bMin, const float* bMax)
{
return
aMin[0] <= bMax[0] && aMax[0] >= bMin[0] &&
aMin[1] <= bMax[1] && aMax[1] >= bMin[1] &&
aMin[2] <= bMax[2] && aMax[2] >= bMin[2];
}
/// Allocates a new span in the heightfield.
/// Use a memory pool and free list to minimize actual allocations.
///
/// @param[in] hf The heightfield
/// @returns A pointer to the allocated or re-used span memory.
static rcSpan* allocSpan(rcHeightfield& hf)
{
// If necessary, allocate new page and update the freelist.
if (hf.freelist == NULL || hf.freelist->next == NULL)
{
// Create new page.
// Allocate memory for the new pool.
rcSpanPool* spanPool = (rcSpanPool*)rcAlloc(sizeof(rcSpanPool), RC_ALLOC_PERM);
if (spanPool == NULL)
{
return NULL;
}
// Add the pool into the list of pools.
spanPool->next = hf.pools;
hf.pools = spanPool;
// Add new spans to the free list.
rcSpan* freeList = hf.freelist;
rcSpan* head = &spanPool->items[0];
rcSpan* it = &spanPool->items[RC_SPANS_PER_POOL];
do
{
--it;
it->next = freeList;
freeList = it;
}
while (it != head);
hf.freelist = it;
}
// Pop item from the front of the free list.
rcSpan* newSpan = hf.freelist;
hf.freelist = hf.freelist->next;
return newSpan;
}
/// Releases the memory used by the span back to the heightfield, so it can be re-used for new spans.
/// @param[in] hf The heightfield.
/// @param[in] span A pointer to the span to free
static void freeSpan(rcHeightfield& hf, rcSpan* span)
{
if (span == NULL)
{
return;
}
// Add the span to the front of the free list.
span->next = hf.freelist;
hf.freelist = span;
}
/// Adds a span to the heightfield. If the new span overlaps existing spans,
/// it will merge the new span with the existing ones.
///
/// @param[in] hf Heightfield to add spans to
/// @param[in] x The new span's column cell x index
/// @param[in] z The new span's column cell z index
/// @param[in] min The new span's minimum cell index
/// @param[in] max The new span's maximum cell index
/// @param[in] areaID The new span's area type ID
/// @param[in] flagMergeThreshold How close two spans maximum extents need to be to merge area type IDs
static bool addSpan(rcHeightfield& hf,
const int x, const int z,
const unsigned short min, const unsigned short max,
const unsigned char areaID, const int flagMergeThreshold)
{
// Create the new span.
rcSpan* newSpan = allocSpan(hf);
if (newSpan == NULL)
{
return false;
}
newSpan->smin = min;
newSpan->smax = max;
newSpan->area = areaID;
newSpan->next = NULL;
const int columnIndex = x + z * hf.width;
rcSpan* previousSpan = NULL;
rcSpan* currentSpan = hf.spans[columnIndex];
// Insert the new span, possibly merging it with existing spans.
while (currentSpan != NULL)
{
if (currentSpan->smin > newSpan->smax)
{
// Current span is completely after the new span, break.
break;
}
if (currentSpan->smax < newSpan->smin)
{
// Current span is completely before the new span. Keep going.
previousSpan = currentSpan;
currentSpan = currentSpan->next;
}
else
{
// The new span overlaps with an existing span. Merge them.
if (currentSpan->smin < newSpan->smin)
{
newSpan->smin = currentSpan->smin;
}
if (currentSpan->smax > newSpan->smax)
{
newSpan->smax = currentSpan->smax;
}
// Merge flags.
if (rcAbs((int)newSpan->smax - (int)currentSpan->smax) <= flagMergeThreshold)
{
// Higher area ID numbers indicate higher resolution priority.
newSpan->area = rcMax(newSpan->area, currentSpan->area);
}
// Remove the current span since it's now merged with newSpan.
// Keep going because there might be other overlapping spans that also need to be merged.
rcSpan* next = currentSpan->next;
freeSpan(hf, currentSpan);
if (previousSpan)
{
previousSpan->next = next;
}
else
{
hf.spans[columnIndex] = next;
}
currentSpan = next;
}
}
// Insert new span after prev
if (previousSpan != NULL)
{
newSpan->next = previousSpan->next;
previousSpan->next = newSpan;
}
else
{
// This span should go before the others in the list
newSpan->next = hf.spans[columnIndex];
hf.spans[columnIndex] = newSpan;
}
return true;
}
bool rcAddSpan(rcContext* context, rcHeightfield& heightfield,
const int x, const int z,
const unsigned short spanMin, const unsigned short spanMax,
const unsigned char areaID, const int flagMergeThreshold)
{
rcAssert(context);
if (!addSpan(heightfield, x, z, spanMin, spanMax, areaID, flagMergeThreshold))
{
context->log(RC_LOG_ERROR, "rcAddSpan: Out of memory.");
return false;
}
return true;
}
enum rcAxis
{
RC_AXIS_X = 0,
RC_AXIS_Y = 1,
RC_AXIS_Z = 2
};
/// Divides a convex polygon of max 12 vertices into two convex polygons
/// across a separating axis.
///
/// @param[in] inVerts The input polygon vertices
/// @param[in] inVertsCount The number of input polygon vertices
/// @param[out] outVerts1 Resulting polygon 1's vertices
/// @param[out] outVerts1Count The number of resulting polygon 1 vertices
/// @param[out] outVerts2 Resulting polygon 2's vertices
/// @param[out] outVerts2Count The number of resulting polygon 2 vertices
/// @param[in] axisOffset THe offset along the specified axis
/// @param[in] axis The separating axis
static void dividePoly(const float* inVerts, int inVertsCount,
float* outVerts1, int* outVerts1Count,
float* outVerts2, int* outVerts2Count,
float axisOffset, rcAxis axis)
{
rcAssert(inVertsCount <= 12);
// How far positive or negative away from the separating axis is each vertex.
float inVertAxisDelta[12];
for (int inVert = 0; inVert < inVertsCount; ++inVert)
{
inVertAxisDelta[inVert] = axisOffset - inVerts[inVert * 3 + axis];
}
int poly1Vert = 0;
int poly2Vert = 0;
for (int inVertA = 0, inVertB = inVertsCount - 1; inVertA < inVertsCount; inVertB = inVertA, ++inVertA)
{
// If the two vertices are on the same side of the separating axis
bool sameSide = (inVertAxisDelta[inVertA] >= 0) == (inVertAxisDelta[inVertB] >= 0);
if (!sameSide)
{
float s = inVertAxisDelta[inVertB] / (inVertAxisDelta[inVertB] - inVertAxisDelta[inVertA]);
outVerts1[poly1Vert * 3 + 0] = inVerts[inVertB * 3 + 0] + (inVerts[inVertA * 3 + 0] - inVerts[inVertB * 3 + 0]) * s;
outVerts1[poly1Vert * 3 + 1] = inVerts[inVertB * 3 + 1] + (inVerts[inVertA * 3 + 1] - inVerts[inVertB * 3 + 1]) * s;
outVerts1[poly1Vert * 3 + 2] = inVerts[inVertB * 3 + 2] + (inVerts[inVertA * 3 + 2] - inVerts[inVertB * 3 + 2]) * s;
rcVcopy(&outVerts2[poly2Vert * 3], &outVerts1[poly1Vert * 3]);
poly1Vert++;
poly2Vert++;
// add the inVertA point to the right polygon. Do NOT add points that are on the dividing line
// since these were already added above
if (inVertAxisDelta[inVertA] > 0)
{
rcVcopy(&outVerts1[poly1Vert * 3], &inVerts[inVertA * 3]);
poly1Vert++;
}
else if (inVertAxisDelta[inVertA] < 0)
{
rcVcopy(&outVerts2[poly2Vert * 3], &inVerts[inVertA * 3]);
poly2Vert++;
}
}
else
{
// add the inVertA point to the right polygon. Addition is done even for points on the dividing line
if (inVertAxisDelta[inVertA] >= 0)
{
rcVcopy(&outVerts1[poly1Vert * 3], &inVerts[inVertA * 3]);
poly1Vert++;
if (inVertAxisDelta[inVertA] != 0)
{
continue;
}
}
rcVcopy(&outVerts2[poly2Vert * 3], &inVerts[inVertA * 3]);
poly2Vert++;
}
}
*outVerts1Count = poly1Vert;
*outVerts2Count = poly2Vert;
}
/// Rasterize a single triangle to the heightfield.
///
/// This code is extremely hot, so much care should be given to maintaining maximum perf here.
///
/// @param[in] v0 Triangle vertex 0
/// @param[in] v1 Triangle vertex 1
/// @param[in] v2 Triangle vertex 2
/// @param[in] areaID The area ID to assign to the rasterized spans
/// @param[in] hf Heightfield to rasterize into
/// @param[in] hfBBMin The min extents of the heightfield bounding box
/// @param[in] hfBBMax The max extents of the heightfield bounding box
/// @param[in] cellSize The x and z axis size of a voxel in the heightfield
/// @param[in] inverseCellSize 1 / cellSize
/// @param[in] inverseCellHeight 1 / cellHeight
/// @param[in] flagMergeThreshold The threshold in which area flags will be merged
/// @returns true if the operation completes successfully. false if there was an error adding spans to the heightfield.
static bool rasterizeTri(const float* v0, const float* v1, const float* v2,
const unsigned char areaID, rcHeightfield& hf,
const float* hfBBMin, const float* hfBBMax,
const float cellSize, const float inverseCellSize, const float inverseCellHeight,
const int flagMergeThreshold)
{
// Calculate the bounding box of the triangle.
float triBBMin[3];
rcVcopy(triBBMin, v0);
rcVmin(triBBMin, v1);
rcVmin(triBBMin, v2);
float triBBMax[3];
rcVcopy(triBBMax, v0);
rcVmax(triBBMax, v1);
rcVmax(triBBMax, v2);
// If the triangle does not touch the bounding box of the heightfield, skip the triangle.
if (!overlapBounds(triBBMin, triBBMax, hfBBMin, hfBBMax))
{
return true;
}
const int w = hf.width;
const int h = hf.height;
const float by = hfBBMax[1] - hfBBMin[1];
// Calculate the footprint of the triangle on the grid's z-axis
int z0 = (int)((triBBMin[2] - hfBBMin[2]) * inverseCellSize);
int z1 = (int)((triBBMax[2] - hfBBMin[2]) * inverseCellSize);
// use -1 rather than 0 to cut the polygon properly at the start of the tile
z0 = rcClamp(z0, -1, h - 1);
z1 = rcClamp(z1, 0, h - 1);
// Clip the triangle into all grid cells it touches.
float buf[7 * 3 * 4];
float* in = buf;
float* inRow = buf + 7 * 3;
float* p1 = inRow + 7 * 3;
float* p2 = p1 + 7 * 3;
rcVcopy(&in[0], v0);
rcVcopy(&in[1 * 3], v1);
rcVcopy(&in[2 * 3], v2);
int nvRow;
int nvIn = 3;
for (int z = z0; z <= z1; ++z)
{
// Clip polygon to row. Store the remaining polygon as well
const float cellZ = hfBBMin[2] + (float)z * cellSize;
dividePoly(in, nvIn, inRow, &nvRow, p1, &nvIn, cellZ + cellSize, RC_AXIS_Z);
rcSwap(in, p1);
if (nvRow < 3)
{
continue;
}
if (z < 0)
{
continue;
}
// find X-axis bounds of the row
float minX = inRow[0];
float maxX = inRow[0];
for (int vert = 1; vert < nvRow; ++vert)
{
if (minX > inRow[vert * 3])
{
minX = inRow[vert * 3];
}
if (maxX < inRow[vert * 3])
{
maxX = inRow[vert * 3];
}
}
int x0 = (int)((minX - hfBBMin[0]) * inverseCellSize);
int x1 = (int)((maxX - hfBBMin[0]) * inverseCellSize);
if (x1 < 0 || x0 >= w)
{
continue;
}
x0 = rcClamp(x0, -1, w - 1);
x1 = rcClamp(x1, 0, w - 1);
int nv;
int nv2 = nvRow;
for (int x = x0; x <= x1; ++x)
{
// Clip polygon to column. store the remaining polygon as well
const float cx = hfBBMin[0] + (float)x * cellSize;
dividePoly(inRow, nv2, p1, &nv, p2, &nv2, cx + cellSize, RC_AXIS_X);
rcSwap(inRow, p2);
if (nv < 3)
{
continue;
}
if (x < 0)
{
continue;
}
// Calculate min and max of the span.
float spanMin = p1[1];
float spanMax = p1[1];
for (int vert = 1; vert < nv; ++vert)
{
spanMin = rcMin(spanMin, p1[vert * 3 + 1]);
spanMax = rcMax(spanMax, p1[vert * 3 + 1]);
}
spanMin -= hfBBMin[1];
spanMax -= hfBBMin[1];
// Skip the span if it's completely outside the heightfield bounding box
if (spanMax < 0.0f)
{
continue;
}
if (spanMin > by)
{
continue;
}
// Clamp the span to the heightfield bounding box.
if (spanMin < 0.0f)
{
spanMin = 0;
}
if (spanMax > by)
{
spanMax = by;
}
// Snap the span to the heightfield height grid.
unsigned short spanMinCellIndex = (unsigned short)rcClamp((int)floorf(spanMin * inverseCellHeight), 0, RC_SPAN_MAX_HEIGHT);
unsigned short spanMaxCellIndex = (unsigned short)rcClamp((int)ceilf(spanMax * inverseCellHeight), (int)spanMinCellIndex + 1, RC_SPAN_MAX_HEIGHT);
if (!addSpan(hf, x, z, spanMinCellIndex, spanMaxCellIndex, areaID, flagMergeThreshold))
{
return false;
}
}
}
return true;
}
bool rcRasterizeTriangle(rcContext* context,
const float* v0, const float* v1, const float* v2,
const unsigned char areaID, rcHeightfield& heightfield, const int flagMergeThreshold)
{
rcAssert(context != NULL);
rcScopedTimer timer(context, RC_TIMER_RASTERIZE_TRIANGLES);
// Rasterize the single triangle.
const float inverseCellSize = 1.0f / heightfield.cs;
const float inverseCellHeight = 1.0f / heightfield.ch;
if (!rasterizeTri(v0, v1, v2, areaID, heightfield, heightfield.bmin, heightfield.bmax, heightfield.cs, inverseCellSize, inverseCellHeight, flagMergeThreshold))
{
context->log(RC_LOG_ERROR, "rcRasterizeTriangle: Out of memory.");
return false;
}
return true;
}
bool rcRasterizeTriangles(rcContext* context,
const float* verts, const int /*nv*/,
const int* tris, const unsigned char* triAreaIDs, const int numTris,
rcHeightfield& heightfield, const int flagMergeThreshold)
{
rcAssert(context != NULL);
rcScopedTimer timer(context, RC_TIMER_RASTERIZE_TRIANGLES);
// Rasterize the triangles.
const float inverseCellSize = 1.0f / heightfield.cs;
const float inverseCellHeight = 1.0f / heightfield.ch;
for (int triIndex = 0; triIndex < numTris; ++triIndex)
{
const float* v0 = &verts[tris[triIndex * 3 + 0] * 3];
const float* v1 = &verts[tris[triIndex * 3 + 1] * 3];
const float* v2 = &verts[tris[triIndex * 3 + 2] * 3];
if (!rasterizeTri(v0, v1, v2, triAreaIDs[triIndex], heightfield, heightfield.bmin, heightfield.bmax, heightfield.cs, inverseCellSize, inverseCellHeight, flagMergeThreshold))
{
context->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
return false;
}
}
return true;
}
bool rcRasterizeTriangles(rcContext* context,
const float* verts, const int /*nv*/,
const unsigned short* tris, const unsigned char* triAreaIDs, const int numTris,
rcHeightfield& heightfield, const int flagMergeThreshold)
{
rcAssert(context != NULL);
rcScopedTimer timer(context, RC_TIMER_RASTERIZE_TRIANGLES);
// Rasterize the triangles.
const float inverseCellSize = 1.0f / heightfield.cs;
const float inverseCellHeight = 1.0f / heightfield.ch;
for (int triIndex = 0; triIndex < numTris; ++triIndex)
{
const float* v0 = &verts[tris[triIndex * 3 + 0] * 3];
const float* v1 = &verts[tris[triIndex * 3 + 1] * 3];
const float* v2 = &verts[tris[triIndex * 3 + 2] * 3];
if (!rasterizeTri(v0, v1, v2, triAreaIDs[triIndex], heightfield, heightfield.bmin, heightfield.bmax, heightfield.cs, inverseCellSize, inverseCellHeight, flagMergeThreshold))
{
context->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
return false;
}
}
return true;
}
bool rcRasterizeTriangles(rcContext* context,
const float* verts, const unsigned char* triAreaIDs, const int numTris,
rcHeightfield& heightfield, const int flagMergeThreshold)
{
rcAssert(context != NULL);
rcScopedTimer timer(context, RC_TIMER_RASTERIZE_TRIANGLES);
// Rasterize the triangles.
const float inverseCellSize = 1.0f / heightfield.cs;
const float inverseCellHeight = 1.0f / heightfield.ch;
for (int triIndex = 0; triIndex < numTris; ++triIndex)
{
const float* v0 = &verts[(triIndex * 3 + 0) * 3];
const float* v1 = &verts[(triIndex * 3 + 1) * 3];
const float* v2 = &verts[(triIndex * 3 + 2) * 3];
if (!rasterizeTri(v0, v1, v2, triAreaIDs[triIndex], heightfield, heightfield.bmin, heightfield.bmax, heightfield.cs, inverseCellSize, inverseCellHeight, flagMergeThreshold))
{
context->log(RC_LOG_ERROR, "rcRasterizeTriangles: Out of memory.");
return false;
}
}
return true;
}

1811
thirdparty/recastnavigation/Recast/Source/RecastRegion.cpp vendored
View File

File diff suppressed because it is too large Load Diff

202
thirdparty/rvo2/LICENSE vendored
View File

@ -1,202 +0,0 @@
Apache License
Version 2.0, January 2004
http://www.apache.org/licenses/
TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
1. Definitions.
"License" shall mean the terms and conditions for use, reproduction,
and distribution as defined by Sections 1 through 9 of this document.
"Licensor" shall mean the copyright owner or entity authorized by
the copyright owner that is granting the License.
"Legal Entity" shall mean the union of the acting entity and all
other entities that control, are controlled by, or are under common
control with that entity. For the purposes of this definition,
"control" means (i) the power, direct or indirect, to cause the
direction or management of such entity, whether by contract or
otherwise, or (ii) ownership of fifty percent (50%) or more of the
outstanding shares, or (iii) beneficial ownership of such entity.
"You" (or "Your") shall mean an individual or Legal Entity
exercising permissions granted by this License.
"Source" form shall mean the preferred form for making modifications,
including but not limited to software source code, documentation
source, and configuration files.
"Object" form shall mean any form resulting from mechanical
transformation or translation of a Source form, including but
not limited to compiled object code, generated documentation,
and conversions to other media types.
"Work" shall mean the work of authorship, whether in Source or
Object form, made available under the License, as indicated by a
copyright notice that is included in or attached to the work
(an example is provided in the Appendix below).
"Derivative Works" shall mean any work, whether in Source or Object
form, that is based on (or derived from) the Work and for which the
editorial revisions, annotations, elaborations, or other modifications
represent, as a whole, an original work of authorship. For the purposes
of this License, Derivative Works shall not include works that remain
separable from, or merely link (or bind by name) to the interfaces of,
the Work and Derivative Works thereof.
"Contribution" shall mean any work of authorship, including
the original version of the Work and any modifications or additions
to that Work or Derivative Works thereof, that is intentionally
submitted to Licensor for inclusion in the Work by the copyright owner
or by an individual or Legal Entity authorized to submit on behalf of
the copyright owner. For the purposes of this definition, "submitted"
means any form of electronic, verbal, or written communication sent
to the Licensor or its representatives, including but not limited to
communication on electronic mailing lists, source code control systems,
and issue tracking systems that are managed by, or on behalf of, the
Licensor for the purpose of discussing and improving the Work, but
excluding communication that is conspicuously marked or otherwise
designated in writing by the copyright owner as "Not a Contribution."
"Contributor" shall mean Licensor and any individual or Legal Entity
on behalf of whom a Contribution has been received by Licensor and
subsequently incorporated within the Work.
2. Grant of Copyright License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
copyright license to reproduce, prepare Derivative Works of,
publicly display, publicly perform, sublicense, and distribute the
Work and such Derivative Works in Source or Object form.
3. Grant of Patent License. Subject to the terms and conditions of
this License, each Contributor hereby grants to You a perpetual,
worldwide, non-exclusive, no-charge, royalty-free, irrevocable
(except as stated in this section) patent license to make, have made,
use, offer to sell, sell, import, and otherwise transfer the Work,
where such license applies only to those patent claims licensable
by such Contributor that are necessarily infringed by their
Contribution(s) alone or by combination of their Contribution(s)
with the Work to which such Contribution(s) was submitted. If You
institute patent litigation against any entity (including a
cross-claim or counterclaim in a lawsuit) alleging that the Work
or a Contribution incorporated within the Work constitutes direct
or contributory patent infringement, then any patent licenses
granted to You under this License for that Work shall terminate
as of the date such litigation is filed.
4. Redistribution. You may reproduce and distribute copies of the
Work or Derivative Works thereof in any medium, with or without
modifications, and in Source or Object form, provided that You
meet the following conditions:
(a) You must give any other recipients of the Work or
Derivative Works a copy of this License; and
(b) You must cause any modified files to carry prominent notices
stating that You changed the files; and
(c) You must retain, in the Source form of any Derivative Works
that You distribute, all copyright, patent, trademark, and
attribution notices from the Source form of the Work,
excluding those notices that do not pertain to any part of
the Derivative Works; and
(d) If the Work includes a "NOTICE" text file as part of its
distribution, then any Derivative Works that You distribute must
include a readable copy of the attribution notices contained
within such NOTICE file, excluding those notices that do not
pertain to any part of the Derivative Works, in at least one
of the following places: within a NOTICE text file distributed
as part of the Derivative Works; within the Source form or
documentation, if provided along with the Derivative Works; or,
within a display generated by the Derivative Works, if and
wherever such third-party notices normally appear. The contents
of the NOTICE file are for informational purposes only and
do not modify the License. You may add Your own attribution
notices within Derivative Works that You distribute, alongside
or as an addendum to the NOTICE text from the Work, provided
that such additional attribution notices cannot be construed
as modifying the License.
You may add Your own copyright statement to Your modifications and
may provide additional or different license terms and conditions
for use, reproduction, or distribution of Your modifications, or
for any such Derivative Works as a whole, provided Your use,
reproduction, and distribution of the Work otherwise complies with
the conditions stated in this License.
5. Submission of Contributions. Unless You explicitly state otherwise,
any Contribution intentionally submitted for inclusion in the Work
by You to the Licensor shall be under the terms and conditions of
this License, without any additional terms or conditions.
Notwithstanding the above, nothing herein shall supersede or modify
the terms of any separate license agreement you may have executed
with Licensor regarding such Contributions.
6. Trademarks. This License does not grant permission to use the trade
names, trademarks, service marks, or product names of the Licensor,
except as required for reasonable and customary use in describing the
origin of the Work and reproducing the content of the NOTICE file.
7. Disclaimer of Warranty. Unless required by applicable law or
agreed to in writing, Licensor provides the Work (and each
Contributor provides its Contributions) on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
implied, including, without limitation, any warranties or conditions
of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
PARTICULAR PURPOSE. You are solely responsible for determining the
appropriateness of using or redistributing the Work and assume any
risks associated with Your exercise of permissions under this License.
8. Limitation of Liability. In no event and under no legal theory,
whether in tort (including negligence), contract, or otherwise,
unless required by applicable law (such as deliberate and grossly
negligent acts) or agreed to in writing, shall any Contributor be
liable to You for damages, including any direct, indirect, special,
incidental, or consequential damages of any character arising as a
result of this License or out of the use or inability to use the
Work (including but not limited to damages for loss of goodwill,
work stoppage, computer failure or malfunction, or any and all
other commercial damages or losses), even if such Contributor
has been advised of the possibility of such damages.
9. Accepting Warranty or Additional Liability. While redistributing
the Work or Derivative Works thereof, You may choose to offer,
and charge a fee for, acceptance of support, warranty, indemnity,
or other liability obligations and/or rights consistent with this
License. However, in accepting such obligations, You may act only
on Your own behalf and on Your sole responsibility, not on behalf
of any other Contributor, and only if You agree to indemnify,
defend, and hold each Contributor harmless for any liability
incurred by, or claims asserted against, such Contributor by reason
of your accepting any such warranty or additional liability.
END OF TERMS AND CONDITIONS
APPENDIX: How to apply the Apache License to your work.
To apply the Apache License to your work, attach the following
boilerplate notice, with the fields enclosed by brackets "[]"
replaced with your own identifying information. (Don't include
the brackets!) The text should be enclosed in the appropriate
comment syntax for the file format. We also recommend that a
file or class name and description of purpose be included on the
same "printed page" as the copyright notice for easier
identification within third-party archives.
Copyright [yyyy] [name of copyright owner]
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.

594
thirdparty/rvo2/rvo2_2d/Agent2d.cpp vendored
View File

@ -1,594 +0,0 @@
/*
* Agent2d.cpp
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#include "Agent2d.h"
#include "KdTree2d.h"
#include "Obstacle2d.h"
namespace RVO2D {
Agent2D::Agent2D() : maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f), timeHorizonObst_(0.0f), id_(0) { }
void Agent2D::computeNeighbors(RVOSimulator2D *sim_)
{
obstacleNeighbors_.clear();
float rangeSq = sqr(timeHorizonObst_ * maxSpeed_ + radius_);
sim_->kdTree_->computeObstacleNeighbors(this, rangeSq);
agentNeighbors_.clear();
if (maxNeighbors_ > 0) {
rangeSq = sqr(neighborDist_);
sim_->kdTree_->computeAgentNeighbors(this, rangeSq);
}
}
/* Search for the best new velocity. */
void Agent2D::computeNewVelocity(RVOSimulator2D *sim_)
{
orcaLines_.clear();
const float invTimeHorizonObst = 1.0f / timeHorizonObst_;
/* Create obstacle ORCA lines. */
for (size_t i = 0; i < obstacleNeighbors_.size(); ++i) {
const Obstacle2D *obstacle1 = obstacleNeighbors_[i].second;
const Obstacle2D *obstacle2 = obstacle1->nextObstacle_;
const Vector2 relativePosition1 = obstacle1->point_ - position_;
const Vector2 relativePosition2 = obstacle2->point_ - position_;
/*
* Check if velocity obstacle of obstacle is already taken care of by
* previously constructed obstacle ORCA lines.
*/
bool alreadyCovered = false;
for (size_t j = 0; j < orcaLines_.size(); ++j) {
if (det(invTimeHorizonObst * relativePosition1 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON && det(invTimeHorizonObst * relativePosition2 - orcaLines_[j].point, orcaLines_[j].direction) - invTimeHorizonObst * radius_ >= -RVO_EPSILON) {
alreadyCovered = true;
break;
}
}
if (alreadyCovered) {
continue;
}
/* Not yet covered. Check for collisions. */
const float distSq1 = absSq(relativePosition1);
const float distSq2 = absSq(relativePosition2);
const float radiusSq = sqr(radius_);
const Vector2 obstacleVector = obstacle2->point_ - obstacle1->point_;
const float s = (-relativePosition1 * obstacleVector) / absSq(obstacleVector);
const float distSqLine = absSq(-relativePosition1 - s * obstacleVector);
Line line;
if (s < 0.0f && distSq1 <= radiusSq) {
/* Collision with left vertex. Ignore if non-convex. */
if (obstacle1->isConvex_) {
line.point = Vector2(0.0f, 0.0f);
line.direction = normalize(Vector2(-relativePosition1.y(), relativePosition1.x()));
orcaLines_.push_back(line);
}
continue;
}
else if (s > 1.0f && distSq2 <= radiusSq) {
/* Collision with right vertex. Ignore if non-convex
* or if it will be taken care of by neighoring obstace */
if (obstacle2->isConvex_ && det(relativePosition2, obstacle2->unitDir_) >= 0.0f) {
line.point = Vector2(0.0f, 0.0f);
line.direction = normalize(Vector2(-relativePosition2.y(), relativePosition2.x()));
orcaLines_.push_back(line);
}
continue;
}
else if (s >= 0.0f && s < 1.0f && distSqLine <= radiusSq) {
/* Collision with obstacle segment. */
line.point = Vector2(0.0f, 0.0f);
line.direction = -obstacle1->unitDir_;
orcaLines_.push_back(line);
continue;
}
/*
* No collision.
* Compute legs. When obliquely viewed, both legs can come from a single
* vertex. Legs extend cut-off line when nonconvex vertex.
*/
Vector2 leftLegDirection, rightLegDirection;
if (s < 0.0f && distSqLine <= radiusSq) {
/*
* Obstacle viewed obliquely so that left vertex
* defines velocity obstacle.
*/
if (!obstacle1->isConvex_) {
/* Ignore obstacle. */
continue;
}
obstacle2 = obstacle1;
const float leg1 = std::sqrt(distSq1 - radiusSq);
leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
rightLegDirection = Vector2(relativePosition1.x() * leg1 + relativePosition1.y() * radius_, -relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
}
else if (s > 1.0f && distSqLine <= radiusSq) {
/*
* Obstacle viewed obliquely so that
* right vertex defines velocity obstacle.
*/
if (!obstacle2->isConvex_) {
/* Ignore obstacle. */
continue;
}
obstacle1 = obstacle2;
const float leg2 = std::sqrt(distSq2 - radiusSq);
leftLegDirection = Vector2(relativePosition2.x() * leg2 - relativePosition2.y() * radius_, relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
}
else {
/* Usual situation. */
if (obstacle1->isConvex_) {
const float leg1 = std::sqrt(distSq1 - radiusSq);
leftLegDirection = Vector2(relativePosition1.x() * leg1 - relativePosition1.y() * radius_, relativePosition1.x() * radius_ + relativePosition1.y() * leg1) / distSq1;
}
else {
/* Left vertex non-convex; left leg extends cut-off line. */
leftLegDirection = -obstacle1->unitDir_;
}
if (obstacle2->isConvex_) {
const float leg2 = std::sqrt(distSq2 - radiusSq);
rightLegDirection = Vector2(relativePosition2.x() * leg2 + relativePosition2.y() * radius_, -relativePosition2.x() * radius_ + relativePosition2.y() * leg2) / distSq2;
}
else {
/* Right vertex non-convex; right leg extends cut-off line. */
rightLegDirection = obstacle1->unitDir_;
}
}
/*
* Legs can never point into neighboring edge when convex vertex,
* take cutoff-line of neighboring edge instead. If velocity projected on
* "foreign" leg, no constraint is added.
*/
const Obstacle2D *const leftNeighbor = obstacle1->prevObstacle_;
bool isLeftLegForeign = false;
bool isRightLegForeign = false;
if (obstacle1->isConvex_ && det(leftLegDirection, -leftNeighbor->unitDir_) >= 0.0f) {
/* Left leg points into obstacle. */
leftLegDirection = -leftNeighbor->unitDir_;
isLeftLegForeign = true;
}
if (obstacle2->isConvex_ && det(rightLegDirection, obstacle2->unitDir_) <= 0.0f) {
/* Right leg points into obstacle. */
rightLegDirection = obstacle2->unitDir_;
isRightLegForeign = true;
}
/* Compute cut-off centers. */
const Vector2 leftCutoff = invTimeHorizonObst * (obstacle1->point_ - position_);
const Vector2 rightCutoff = invTimeHorizonObst * (obstacle2->point_ - position_);
const Vector2 cutoffVec = rightCutoff - leftCutoff;
/* Project current velocity on velocity obstacle. */
/* Check if current velocity is projected on cutoff circles. */
const float t = (obstacle1 == obstacle2 ? 0.5f : ((velocity_ - leftCutoff) * cutoffVec) / absSq(cutoffVec));
const float tLeft = ((velocity_ - leftCutoff) * leftLegDirection);
const float tRight = ((velocity_ - rightCutoff) * rightLegDirection);
if ((t < 0.0f && tLeft < 0.0f) || (obstacle1 == obstacle2 && tLeft < 0.0f && tRight < 0.0f)) {
/* Project on left cut-off circle. */
const Vector2 unitW = normalize(velocity_ - leftCutoff);
line.direction = Vector2(unitW.y(), -unitW.x());
line.point = leftCutoff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.push_back(line);
continue;
}
else if (t > 1.0f && tRight < 0.0f) {
/* Project on right cut-off circle. */
const Vector2 unitW = normalize(velocity_ - rightCutoff);
line.direction = Vector2(unitW.y(), -unitW.x());
line.point = rightCutoff + radius_ * invTimeHorizonObst * unitW;
orcaLines_.push_back(line);
continue;
}
/*
* Project on left leg, right leg, or cut-off line, whichever is closest
* to velocity.
*/
const float distSqCutoff = ((t < 0.0f || t > 1.0f || obstacle1 == obstacle2) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + t * cutoffVec)));
const float distSqLeft = ((tLeft < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (leftCutoff + tLeft * leftLegDirection)));
const float distSqRight = ((tRight < 0.0f) ? std::numeric_limits<float>::infinity() : absSq(velocity_ - (rightCutoff + tRight * rightLegDirection)));
if (distSqCutoff <= distSqLeft && distSqCutoff <= distSqRight) {
/* Project on cut-off line. */
line.direction = -obstacle1->unitDir_;
line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
orcaLines_.push_back(line);
continue;
}
else if (distSqLeft <= distSqRight) {
/* Project on left leg. */
if (isLeftLegForeign) {
continue;
}
line.direction = leftLegDirection;
line.point = leftCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
orcaLines_.push_back(line);
continue;
}
else {
/* Project on right leg. */
if (isRightLegForeign) {
continue;
}
line.direction = -rightLegDirection;
line.point = rightCutoff + radius_ * invTimeHorizonObst * Vector2(-line.direction.y(), line.direction.x());
orcaLines_.push_back(line);
continue;
}
}
const size_t numObstLines = orcaLines_.size();
const float invTimeHorizon = 1.0f / timeHorizon_;
/* Create agent ORCA lines. */
for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
const Agent2D *const other = agentNeighbors_[i].second;
//const float timeHorizon_mod = (avoidance_priority_ - other->avoidance_priority_ + 1.0f) * 0.5f;
//const float invTimeHorizon = (1.0f / timeHorizon_) * timeHorizon_mod;
const Vector2 relativePosition = other->position_ - position_;
const Vector2 relativeVelocity = velocity_ - other->velocity_;
const float distSq = absSq(relativePosition);
const float combinedRadius = radius_ + other->radius_;
const float combinedRadiusSq = sqr(combinedRadius);
Line line;
Vector2 u;
if (distSq > combinedRadiusSq) {
/* No collision. */
const Vector2 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
const float wLengthSq = absSq(w);
const float dotProduct1 = w * relativePosition;
if (dotProduct1 < 0.0f && sqr(dotProduct1) > combinedRadiusSq * wLengthSq) {
/* Project on cut-off circle. */
const float wLength = std::sqrt(wLengthSq);
const Vector2 unitW = w / wLength;
line.direction = Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else {
/* Project on legs. */
const float leg = std::sqrt(distSq - combinedRadiusSq);
if (det(relativePosition, w) > 0.0f) {
/* Project on left leg. */
line.direction = Vector2(relativePosition.x() * leg - relativePosition.y() * combinedRadius, relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
else {
/* Project on right leg. */
line.direction = -Vector2(relativePosition.x() * leg + relativePosition.y() * combinedRadius, -relativePosition.x() * combinedRadius + relativePosition.y() * leg) / distSq;
}
const float dotProduct2 = relativeVelocity * line.direction;
u = dotProduct2 * line.direction - relativeVelocity;
}
}
else {
/* Collision. Project on cut-off circle of time timeStep. */
const float invTimeStep = 1.0f / sim_->timeStep_;
/* Vector from cutoff center to relative velocity. */
const Vector2 w = relativeVelocity - invTimeStep * relativePosition;
const float wLength = abs(w);
const Vector2 unitW = w / wLength;
line.direction = Vector2(unitW.y(), -unitW.x());
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
line.point = velocity_ + 0.5f * u;
orcaLines_.push_back(line);
}
size_t lineFail = linearProgram2(orcaLines_, maxSpeed_, prefVelocity_, false, newVelocity_);
if (lineFail < orcaLines_.size()) {
linearProgram3(orcaLines_, numObstLines, lineFail, maxSpeed_, newVelocity_);
}
}
void Agent2D::insertAgentNeighbor(const Agent2D *agent, float &rangeSq)
{
// no point processing same agent
if (this == agent) {
return;
}
// ignore other agent if layers/mask bitmasks have no matching bit
if ((avoidance_mask_ & agent->avoidance_layers_) == 0) {
return;
}
// ignore other agent if this agent is below or above
if ((elevation_ > agent->elevation_ + agent->height_) || (elevation_ + height_ < agent->elevation_)) {
return;
}
if (avoidance_priority_ > agent->avoidance_priority_) {
return;
}
const float distSq = absSq(position_ - agent->position_);
if (distSq < rangeSq) {
if (agentNeighbors_.size() < maxNeighbors_) {
agentNeighbors_.push_back(std::make_pair(distSq, agent));
}
size_t i = agentNeighbors_.size() - 1;
while (i != 0 && distSq < agentNeighbors_[i - 1].first) {
agentNeighbors_[i] = agentNeighbors_[i - 1];
--i;
}
agentNeighbors_[i] = std::make_pair(distSq, agent);
if (agentNeighbors_.size() == maxNeighbors_) {
rangeSq = agentNeighbors_.back().first;
}
}
}
void Agent2D::insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq)
{
const Obstacle2D *const nextObstacle = obstacle->nextObstacle_;
// ignore obstacle if no matching layer/mask
if ((avoidance_mask_ & nextObstacle->avoidance_layers_) == 0) {
return;
}
// ignore obstacle if below or above
if ((elevation_ > obstacle->elevation_ + obstacle->height_) || (elevation_ + height_ < obstacle->elevation_)) {
return;
}
const float distSq = distSqPointLineSegment(obstacle->point_, nextObstacle->point_, position_);
if (distSq < rangeSq) {
obstacleNeighbors_.push_back(std::make_pair(distSq, obstacle));
size_t i = obstacleNeighbors_.size() - 1;
while (i != 0 && distSq < obstacleNeighbors_[i - 1].first) {
obstacleNeighbors_[i] = obstacleNeighbors_[i - 1];
--i;
}
obstacleNeighbors_[i] = std::make_pair(distSq, obstacle);
}
//}
}
void Agent2D::update(RVOSimulator2D *sim_)
{
velocity_ = newVelocity_;
position_ += velocity_ * sim_->timeStep_;
}
bool linearProgram1(const std::vector<Line> &lines, size_t lineNo, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)
{
const float dotProduct = lines[lineNo].point * lines[lineNo].direction;
const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(lines[lineNo].point);
if (discriminant < 0.0f) {
/* Max speed circle fully invalidates line lineNo. */
return false;
}
const float sqrtDiscriminant = std::sqrt(discriminant);
float tLeft = -dotProduct - sqrtDiscriminant;
float tRight = -dotProduct + sqrtDiscriminant;
for (size_t i = 0; i < lineNo; ++i) {
const float denominator = det(lines[lineNo].direction, lines[i].direction);
const float numerator = det(lines[i].direction, lines[lineNo].point - lines[i].point);
if (std::fabs(denominator) <= RVO_EPSILON) {
/* Lines lineNo and i are (almost) parallel. */
if (numerator < 0.0f) {
return false;
}
else {
continue;
}
}
const float t = numerator / denominator;
if (denominator >= 0.0f) {
/* Line i bounds line lineNo on the right. */
tRight = std::min(tRight, t);
}
else {
/* Line i bounds line lineNo on the left. */
tLeft = std::max(tLeft, t);
}
if (tLeft > tRight) {
return false;
}
}
if (directionOpt) {
/* Optimize direction. */
if (optVelocity * lines[lineNo].direction > 0.0f) {
/* Take right extreme. */
result = lines[lineNo].point + tRight * lines[lineNo].direction;
}
else {
/* Take left extreme. */
result = lines[lineNo].point + tLeft * lines[lineNo].direction;
}
}
else {
/* Optimize closest point. */
const float t = lines[lineNo].direction * (optVelocity - lines[lineNo].point);
if (t < tLeft) {
result = lines[lineNo].point + tLeft * lines[lineNo].direction;
}
else if (t > tRight) {
result = lines[lineNo].point + tRight * lines[lineNo].direction;
}
else {
result = lines[lineNo].point + t * lines[lineNo].direction;
}
}
return true;
}
size_t linearProgram2(const std::vector<Line> &lines, float radius, const Vector2 &optVelocity, bool directionOpt, Vector2 &result)
{
if (directionOpt) {
/*
* Optimize direction. Note that the optimization velocity is of unit
* length in this case.
*/
result = optVelocity * radius;
}
else if (absSq(optVelocity) > sqr(radius)) {
/* Optimize closest point and outside circle. */
result = normalize(optVelocity) * radius;
}
else {
/* Optimize closest point and inside circle. */
result = optVelocity;
}
for (size_t i = 0; i < lines.size(); ++i) {
if (det(lines[i].direction, lines[i].point - result) > 0.0f) {
/* Result does not satisfy constraint i. Compute new optimal result. */
const Vector2 tempResult = result;
if (!linearProgram1(lines, i, radius, optVelocity, directionOpt, result)) {
result = tempResult;
return i;
}
}
}
return lines.size();
}
void linearProgram3(const std::vector<Line> &lines, size_t numObstLines, size_t beginLine, float radius, Vector2 &result)
{
float distance = 0.0f;
for (size_t i = beginLine; i < lines.size(); ++i) {
if (det(lines[i].direction, lines[i].point - result) > distance) {
/* Result does not satisfy constraint of line i. */
std::vector<Line> projLines(lines.begin(), lines.begin() + static_cast<ptrdiff_t>(numObstLines));
for (size_t j = numObstLines; j < i; ++j) {
Line line;
float determinant = det(lines[i].direction, lines[j].direction);
if (std::fabs(determinant) <= RVO_EPSILON) {
/* Line i and line j are parallel. */
if (lines[i].direction * lines[j].direction > 0.0f) {
/* Line i and line j point in the same direction. */
continue;
}
else {
/* Line i and line j point in opposite direction. */
line.point = 0.5f * (lines[i].point + lines[j].point);
}
}
else {
line.point = lines[i].point + (det(lines[j].direction, lines[i].point - lines[j].point) / determinant) * lines[i].direction;
}
line.direction = normalize(lines[j].direction - lines[i].direction);
projLines.push_back(line);
}
const Vector2 tempResult = result;
if (linearProgram2(projLines, radius, Vector2(-lines[i].direction.y(), lines[i].direction.x()), true, result) < projLines.size()) {
/* This should in principle not happen. The result is by definition
* already in the feasible region of this linear program. If it fails,
* it is due to small floating point error, and the current result is
* kept.
*/
result = tempResult;
}
distance = det(lines[i].direction, lines[i].point - result);
}
}
}
}

160
thirdparty/rvo2/rvo2_2d/Agent2d.h vendored
View File

@ -1,160 +0,0 @@
/*
* Agent2d.h
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#ifndef RVO2D_AGENT_H_
#define RVO2D_AGENT_H_
/**
* \file Agent2d.h
* \brief Contains the Agent class.
*/
#include "Definitions.h"
#include "RVOSimulator2d.h"
namespace RVO2D {
/**
* \brief Defines an agent in the simulation.
*/
class Agent2D {
public:
/**
* \brief Constructs an agent instance.
* \param sim The simulator instance.
*/
explicit Agent2D();
/**
* \brief Computes the neighbors of this agent.
*/
void computeNeighbors(RVOSimulator2D *sim_);
/**
* \brief Computes the new velocity of this agent.
*/
void computeNewVelocity(RVOSimulator2D *sim_);
/**
* \brief Inserts an agent neighbor into the set of neighbors of
* this agent.
* \param agent A pointer to the agent to be inserted.
* \param rangeSq The squared range around this agent.
*/
void insertAgentNeighbor(const Agent2D *agent, float &rangeSq);
/**
* \brief Inserts a static obstacle neighbor into the set of neighbors
* of this agent.
* \param obstacle The number of the static obstacle to be
* inserted.
* \param rangeSq The squared range around this agent.
*/
void insertObstacleNeighbor(const Obstacle2D *obstacle, float rangeSq);
/**
* \brief Updates the two-dimensional position and two-dimensional
* velocity of this agent.
*/
void update(RVOSimulator2D *sim_);
std::vector<std::pair<float, const Agent2D *> > agentNeighbors_;
size_t maxNeighbors_;
float maxSpeed_;
float neighborDist_;
Vector2 newVelocity_;
std::vector<std::pair<float, const Obstacle2D *> > obstacleNeighbors_;
std::vector<Line> orcaLines_;
Vector2 position_;
Vector2 prefVelocity_;
float radius_;
float timeHorizon_;
float timeHorizonObst_;
Vector2 velocity_;
float height_ = 0.0;
float elevation_ = 0.0;
uint32_t avoidance_layers_ = 1;
uint32_t avoidance_mask_ = 1;
float avoidance_priority_ = 1.0;
size_t id_;
friend class KdTree2D;
friend class RVOSimulator2D;
};
/**
* \relates Agent
* \brief Solves a one-dimensional linear program on a specified line
* subject to linear constraints defined by lines and a circular
* constraint.
* \param lines Lines defining the linear constraints.
* \param lineNo The specified line constraint.
* \param radius The radius of the circular constraint.
* \param optVelocity The optimization velocity.
* \param directionOpt True if the direction should be optimized.
* \param result A reference to the result of the linear program.
* \return True if successful.
*/
bool linearProgram1(const std::vector<Line> &lines, size_t lineNo,
float radius, const Vector2 &optVelocity,
bool directionOpt, Vector2 &result);
/**
* \relates Agent
* \brief Solves a two-dimensional linear program subject to linear
* constraints defined by lines and a circular constraint.
* \param lines Lines defining the linear constraints.
* \param radius The radius of the circular constraint.
* \param optVelocity The optimization velocity.
* \param directionOpt True if the direction should be optimized.
* \param result A reference to the result of the linear program.
* \return The number of the line it fails on, and the number of lines if successful.
*/
size_t linearProgram2(const std::vector<Line> &lines, float radius,
const Vector2 &optVelocity, bool directionOpt,
Vector2 &result);
/**
* \relates Agent
* \brief Solves a two-dimensional linear program subject to linear
* constraints defined by lines and a circular constraint.
* \param lines Lines defining the linear constraints.
* \param numObstLines Count of obstacle lines.
* \param beginLine The line on which the 2-d linear program failed.
* \param radius The radius of the circular constraint.
* \param result A reference to the result of the linear program.
*/
void linearProgram3(const std::vector<Line> &lines, size_t numObstLines, size_t beginLine,
float radius, Vector2 &result);
}
#endif /* RVO2D_AGENT_H_ */

110
thirdparty/rvo2/rvo2_2d/Definitions.h vendored
View File

@ -1,110 +0,0 @@
/*
* Definitions.h
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#ifndef RVO2D_DEFINITIONS_H_
#define RVO2D_DEFINITIONS_H_
/**
* \file Definitions.h
* \brief Contains functions and constants used in multiple classes.
*/
#include <algorithm>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <limits>
#include <vector>
#include "Vector2.h"
/**
* \brief A sufficiently small positive number.
*/
const float RVO_EPSILON = 0.00001f;
namespace RVO2D {
class Agent2D;
class Obstacle2D;
class RVOSimulator2D;
/**
* \brief Computes the squared distance from a line segment with the
* specified endpoints to a specified point.
* \param a The first endpoint of the line segment.
* \param b The second endpoint of the line segment.
* \param c The point to which the squared distance is to
* be calculated.
* \return The squared distance from the line segment to the point.
*/
inline float distSqPointLineSegment(const Vector2 &a, const Vector2 &b,
const Vector2 &c)
{
const float r = ((c - a) * (b - a)) / absSq(b - a);
if (r < 0.0f) {
return absSq(c - a);
}
else if (r > 1.0f) {
return absSq(c - b);
}
else {
return absSq(c - (a + r * (b - a)));
}
}
/**
* \brief Computes the signed distance from a line connecting the
* specified points to a specified point.
* \param a The first point on the line.
* \param b The second point on the line.
* \param c The point to which the signed distance is to
* be calculated.
* \return Positive when the point c lies to the left of the line ab.
*/
inline float leftOf(const Vector2 &a, const Vector2 &b, const Vector2 &c)
{
return det(a - c, b - a);
}
/**
* \brief Computes the square of a float.
* \param a The float to be squared.
* \return The square of the float.
*/
inline float sqr(float a)
{
return a * a;
}
}
#endif /* RVO2D_DEFINITIONS_H_ */

357
thirdparty/rvo2/rvo2_2d/KdTree2d.cpp vendored
View File

@ -1,357 +0,0 @@
/*
* KdTree2d.cpp
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#include "KdTree2d.h"
#include "Agent2d.h"
#include "RVOSimulator2d.h"
#include "Obstacle2d.h"
namespace RVO2D {
KdTree2D::KdTree2D(RVOSimulator2D *sim) : obstacleTree_(NULL), sim_(sim) { }
KdTree2D::~KdTree2D()
{
deleteObstacleTree(obstacleTree_);
}
void KdTree2D::buildAgentTree(std::vector<Agent2D *> agents)
{
agents_.swap(agents);
if (!agents_.empty()) {
agentTree_.resize(2 * agents_.size() - 1);
buildAgentTreeRecursive(0, agents_.size(), 0);
}
}
void KdTree2D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
{
agentTree_[node].begin = begin;
agentTree_[node].end = end;
agentTree_[node].minX = agentTree_[node].maxX = agents_[begin]->position_.x();
agentTree_[node].minY = agentTree_[node].maxY = agents_[begin]->position_.y();
for (size_t i = begin + 1; i < end; ++i) {
agentTree_[node].maxX = std::max(agentTree_[node].maxX, agents_[i]->position_.x());
agentTree_[node].minX = std::min(agentTree_[node].minX, agents_[i]->position_.x());
agentTree_[node].maxY = std::max(agentTree_[node].maxY, agents_[i]->position_.y());
agentTree_[node].minY = std::min(agentTree_[node].minY, agents_[i]->position_.y());
}
if (end - begin > MAX_LEAF_SIZE) {
/* No leaf node. */
const bool isVertical = (agentTree_[node].maxX - agentTree_[node].minX > agentTree_[node].maxY - agentTree_[node].minY);
const float splitValue = (isVertical ? 0.5f * (agentTree_[node].maxX + agentTree_[node].minX) : 0.5f * (agentTree_[node].maxY + agentTree_[node].minY));
size_t left = begin;
size_t right = end;
while (left < right) {
while (left < right && (isVertical ? agents_[left]->position_.x() : agents_[left]->position_.y()) < splitValue) {
++left;
}
while (right > left && (isVertical ? agents_[right - 1]->position_.x() : agents_[right - 1]->position_.y()) >= splitValue) {
--right;
}
if (left < right) {
std::swap(agents_[left], agents_[right - 1]);
++left;
--right;
}
}
if (left == begin) {
++left;
++right;
}
agentTree_[node].left = node + 1;
agentTree_[node].right = node + 2 * (left - begin);
buildAgentTreeRecursive(begin, left, agentTree_[node].left);
buildAgentTreeRecursive(left, end, agentTree_[node].right);
}
}
void KdTree2D::buildObstacleTree(std::vector<Obstacle2D *> obstacles)
{
deleteObstacleTree(obstacleTree_);
obstacleTree_ = buildObstacleTreeRecursive(obstacles);
}
KdTree2D::ObstacleTreeNode *KdTree2D::buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &obstacles)
{
if (obstacles.empty()) {
return NULL;
}
else {
ObstacleTreeNode *const node = new ObstacleTreeNode;
size_t optimalSplit = 0;
size_t minLeft = obstacles.size();
size_t minRight = obstacles.size();
for (size_t i = 0; i < obstacles.size(); ++i) {
size_t leftSize = 0;
size_t rightSize = 0;
const Obstacle2D *const obstacleI1 = obstacles[i];
const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_;
/* Compute optimal split node. */
for (size_t j = 0; j < obstacles.size(); ++j) {
if (i == j) {
continue;
}
const Obstacle2D *const obstacleJ1 = obstacles[j];
const Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_;
const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_);
const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_);
if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) {
++leftSize;
}
else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) {
++rightSize;
}
else {
++leftSize;
++rightSize;
}
if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) >= std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
break;
}
}
if (std::make_pair(std::max(leftSize, rightSize), std::min(leftSize, rightSize)) < std::make_pair(std::max(minLeft, minRight), std::min(minLeft, minRight))) {
minLeft = leftSize;
minRight = rightSize;
optimalSplit = i;
}
}
/* Build split node. */
std::vector<Obstacle2D *> leftObstacles(minLeft);
std::vector<Obstacle2D *> rightObstacles(minRight);
size_t leftCounter = 0;
size_t rightCounter = 0;
const size_t i = optimalSplit;
const Obstacle2D *const obstacleI1 = obstacles[i];
const Obstacle2D *const obstacleI2 = obstacleI1->nextObstacle_;
for (size_t j = 0; j < obstacles.size(); ++j) {
if (i == j) {
continue;
}
Obstacle2D *const obstacleJ1 = obstacles[j];
Obstacle2D *const obstacleJ2 = obstacleJ1->nextObstacle_;
const float j1LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ1->point_);
const float j2LeftOfI = leftOf(obstacleI1->point_, obstacleI2->point_, obstacleJ2->point_);
if (j1LeftOfI >= -RVO_EPSILON && j2LeftOfI >= -RVO_EPSILON) {
leftObstacles[leftCounter++] = obstacles[j];
}
else if (j1LeftOfI <= RVO_EPSILON && j2LeftOfI <= RVO_EPSILON) {
rightObstacles[rightCounter++] = obstacles[j];
}
else {
/* Split obstacle j. */
const float t = det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleI1->point_) / det(obstacleI2->point_ - obstacleI1->point_, obstacleJ1->point_ - obstacleJ2->point_);
const Vector2 splitpoint = obstacleJ1->point_ + t * (obstacleJ2->point_ - obstacleJ1->point_);
Obstacle2D *const newObstacle = new Obstacle2D();
newObstacle->point_ = splitpoint;
newObstacle->prevObstacle_ = obstacleJ1;
newObstacle->nextObstacle_ = obstacleJ2;
newObstacle->isConvex_ = true;
newObstacle->unitDir_ = obstacleJ1->unitDir_;
newObstacle->id_ = sim_->obstacles_.size();
sim_->obstacles_.push_back(newObstacle);
obstacleJ1->nextObstacle_ = newObstacle;
obstacleJ2->prevObstacle_ = newObstacle;
if (j1LeftOfI > 0.0f) {
leftObstacles[leftCounter++] = obstacleJ1;
rightObstacles[rightCounter++] = newObstacle;
}
else {
rightObstacles[rightCounter++] = obstacleJ1;
leftObstacles[leftCounter++] = newObstacle;
}
}
}
node->obstacle = obstacleI1;
node->left = buildObstacleTreeRecursive(leftObstacles);
node->right = buildObstacleTreeRecursive(rightObstacles);
return node;
}
}
void KdTree2D::computeAgentNeighbors(Agent2D *agent, float &rangeSq) const
{
queryAgentTreeRecursive(agent, rangeSq, 0);
}
void KdTree2D::computeObstacleNeighbors(Agent2D *agent, float rangeSq) const
{
queryObstacleTreeRecursive(agent, rangeSq, obstacleTree_);
}
void KdTree2D::deleteObstacleTree(ObstacleTreeNode *node)
{
if (node != NULL) {
deleteObstacleTree(node->left);
deleteObstacleTree(node->right);
delete node;
}
}
void KdTree2D::queryAgentTreeRecursive(Agent2D *agent, float &rangeSq, size_t node) const
{
if (agentTree_[node].end - agentTree_[node].begin <= MAX_LEAF_SIZE) {
for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
agent->insertAgentNeighbor(agents_[i], rangeSq);
}
}
else {
const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxY));
const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minX - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxX)) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minY - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxY));
if (distSqLeft < distSqRight) {
if (distSqLeft < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
if (distSqRight < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
}
}
}
else {
if (distSqRight < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
if (distSqLeft < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
}
}
}
}
}
void KdTree2D::queryObstacleTreeRecursive(Agent2D *agent, float rangeSq, const ObstacleTreeNode *node) const
{
if (node == NULL) {
return;
}
else {
const Obstacle2D *const obstacle1 = node->obstacle;
const Obstacle2D *const obstacle2 = obstacle1->nextObstacle_;
const float agentLeftOfLine = leftOf(obstacle1->point_, obstacle2->point_, agent->position_);
queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->left : node->right));
const float distSqLine = sqr(agentLeftOfLine) / absSq(obstacle2->point_ - obstacle1->point_);
if (distSqLine < rangeSq) {
if (agentLeftOfLine < 0.0f) {
/*
* Try obstacle at this node only if agent is on right side of
* obstacle (and can see obstacle).
*/
agent->insertObstacleNeighbor(node->obstacle, rangeSq);
}
/* Try other side of line. */
queryObstacleTreeRecursive(agent, rangeSq, (agentLeftOfLine >= 0.0f ? node->right : node->left));
}
}
}
bool KdTree2D::queryVisibility(const Vector2 &q1, const Vector2 &q2, float radius) const
{
return queryVisibilityRecursive(q1, q2, radius, obstacleTree_);
}
bool KdTree2D::queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2, float radius, const ObstacleTreeNode *node) const
{
if (node == NULL) {
return true;
}
else {
const Obstacle2D *const obstacle1 = node->obstacle;
const Obstacle2D *const obstacle2 = obstacle1->nextObstacle_;
const float q1LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q1);
const float q2LeftOfI = leftOf(obstacle1->point_, obstacle2->point_, q2);
const float invLengthI = 1.0f / absSq(obstacle2->point_ - obstacle1->point_);
if (q1LeftOfI >= 0.0f && q2LeftOfI >= 0.0f) {
return queryVisibilityRecursive(q1, q2, radius, node->left) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->right));
}
else if (q1LeftOfI <= 0.0f && q2LeftOfI <= 0.0f) {
return queryVisibilityRecursive(q1, q2, radius, node->right) && ((sqr(q1LeftOfI) * invLengthI >= sqr(radius) && sqr(q2LeftOfI) * invLengthI >= sqr(radius)) || queryVisibilityRecursive(q1, q2, radius, node->left));
}
else if (q1LeftOfI >= 0.0f && q2LeftOfI <= 0.0f) {
/* One can see through obstacle from left to right. */
return queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right);
}
else {
const float point1LeftOfQ = leftOf(q1, q2, obstacle1->point_);
const float point2LeftOfQ = leftOf(q1, q2, obstacle2->point_);
const float invLengthQ = 1.0f / absSq(q2 - q1);
return (point1LeftOfQ * point2LeftOfQ >= 0.0f && sqr(point1LeftOfQ) * invLengthQ > sqr(radius) && sqr(point2LeftOfQ) * invLengthQ > sqr(radius) && queryVisibilityRecursive(q1, q2, radius, node->left) && queryVisibilityRecursive(q1, q2, radius, node->right));
}
}
}
}

203
thirdparty/rvo2/rvo2_2d/KdTree2d.h vendored
View File

@ -1,203 +0,0 @@
/*
* KdTree2d.h
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#ifndef RVO2D_KD_TREE_H_
#define RVO2D_KD_TREE_H_
/**
* \file KdTree2d.h
* \brief Contains the KdTree class.
*/
#include "Definitions.h"
namespace RVO2D {
/**
* \brief Defines <i>k</i>d-trees for agents and static obstacles in the
* simulation.
*/
class KdTree2D {
public:
/**
* \brief Defines an agent <i>k</i>d-tree node.
*/
class AgentTreeNode {
public:
/**
* \brief The beginning node number.
*/
size_t begin;
/**
* \brief The ending node number.
*/
size_t end;
/**
* \brief The left node number.
*/
size_t left;
/**
* \brief The maximum x-coordinate.
*/
float maxX;
/**
* \brief The maximum y-coordinate.
*/
float maxY;
/**
* \brief The minimum x-coordinate.
*/
float minX;
/**
* \brief The minimum y-coordinate.
*/
float minY;
/**
* \brief The right node number.
*/
size_t right;
};
/**
* \brief Defines an obstacle <i>k</i>d-tree node.
*/
class ObstacleTreeNode {
public:
/**
* \brief The left obstacle tree node.
*/
ObstacleTreeNode *left;
/**
* \brief The obstacle number.
*/
const Obstacle2D *obstacle;
/**
* \brief The right obstacle tree node.
*/
ObstacleTreeNode *right;
};
/**
* \brief Constructs a <i>k</i>d-tree instance.
* \param sim The simulator instance.
*/
explicit KdTree2D(RVOSimulator2D *sim);
/**
* \brief Destroys this kd-tree instance.
*/
~KdTree2D();
/**
* \brief Builds an agent <i>k</i>d-tree.
*/
void buildAgentTree(std::vector<Agent2D *> agents);
void buildAgentTreeRecursive(size_t begin, size_t end, size_t node);
/**
* \brief Builds an obstacle <i>k</i>d-tree.
*/
void buildObstacleTree(std::vector<Obstacle2D *> obstacles);
ObstacleTreeNode *buildObstacleTreeRecursive(const std::vector<Obstacle2D *> &
obstacles);
/**
* \brief Computes the agent neighbors of the specified agent.
* \param agent A pointer to the agent for which agent
* neighbors are to be computed.
* \param rangeSq The squared range around the agent.
*/
void computeAgentNeighbors(Agent2D *agent, float &rangeSq) const;
/**
* \brief Computes the obstacle neighbors of the specified agent.
* \param agent A pointer to the agent for which obstacle
* neighbors are to be computed.
* \param rangeSq The squared range around the agent.
*/
void computeObstacleNeighbors(Agent2D *agent, float rangeSq) const;
/**
* \brief Deletes the specified obstacle tree node.
* \param node A pointer to the obstacle tree node to be
* deleted.
*/
void deleteObstacleTree(ObstacleTreeNode *node);
void queryAgentTreeRecursive(Agent2D *agent, float &rangeSq,
size_t node) const;
void queryObstacleTreeRecursive(Agent2D *agent, float rangeSq,
const ObstacleTreeNode *node) const;
/**
* \brief Queries the visibility between two points within a
* specified radius.
* \param q1 The first point between which visibility is
* to be tested.
* \param q2 The second point between which visibility is
* to be tested.
* \param radius The radius within which visibility is to be
* tested.
* \return True if q1 and q2 are mutually visible within the radius;
* false otherwise.
*/
bool queryVisibility(const Vector2 &q1, const Vector2 &q2,
float radius) const;
bool queryVisibilityRecursive(const Vector2 &q1, const Vector2 &q2,
float radius,
const ObstacleTreeNode *node) const;
std::vector<Agent2D *> agents_;
std::vector<AgentTreeNode> agentTree_;
ObstacleTreeNode *obstacleTree_;
RVOSimulator2D *sim_;
static const size_t MAX_LEAF_SIZE = 10;
friend class Agent2D;
friend class RVOSimulator2D;
};
}
#endif /* RVO2D_KD_TREE_H_ */

38
thirdparty/rvo2/rvo2_2d/Obstacle2d.cpp vendored
View File

@ -1,38 +0,0 @@
/*
* Obstacle2d.cpp
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#include "Obstacle2d.h"
#include "RVOSimulator2d.h"
namespace RVO2D {
Obstacle2D::Obstacle2D() : isConvex_(false), nextObstacle_(NULL), prevObstacle_(NULL), id_(0) { }
}

72
thirdparty/rvo2/rvo2_2d/Obstacle2d.h vendored
View File

@ -1,72 +0,0 @@
/*
* Obstacle2d.h
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#ifndef RVO2D_OBSTACLE_H_
#define RVO2D_OBSTACLE_H_
/**
* \file Obstacle2d.h
* \brief Contains the Obstacle class.
*/
#include "Definitions.h"
namespace RVO2D {
/**
* \brief Defines static obstacles in the simulation.
*/
class Obstacle2D {
public:
/**
* \brief Constructs a static obstacle instance.
*/
Obstacle2D();
bool isConvex_;
Obstacle2D *nextObstacle_;
Vector2 point_;
Obstacle2D *prevObstacle_;
Vector2 unitDir_;
float height_ = 1.0;
float elevation_ = 0.0;
uint32_t avoidance_layers_ = 1;
size_t id_;
friend class Agent2D;
friend class KdTree2D;
friend class RVOSimulator2D;
};
}
#endif /* RVO2D_OBSTACLE_H_ */

363
thirdparty/rvo2/rvo2_2d/RVOSimulator2d.cpp vendored
View File

@ -1,363 +0,0 @@
/*
* RVOSimulator2d.cpp
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#include "RVOSimulator2d.h"
#include "Agent2d.h"
#include "KdTree2d.h"
#include "Obstacle2d.h"
#ifdef _OPENMP
#include <omp.h>
#endif
namespace RVO2D {
RVOSimulator2D::RVOSimulator2D() : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(0.0f)
{
kdTree_ = new KdTree2D(this);
}
RVOSimulator2D::RVOSimulator2D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity) : defaultAgent_(NULL), globalTime_(0.0f), kdTree_(NULL), timeStep_(timeStep)
{
kdTree_ = new KdTree2D(this);
defaultAgent_ = new Agent2D();
defaultAgent_->maxNeighbors_ = maxNeighbors;
defaultAgent_->maxSpeed_ = maxSpeed;
defaultAgent_->neighborDist_ = neighborDist;
defaultAgent_->radius_ = radius;
defaultAgent_->timeHorizon_ = timeHorizon;
defaultAgent_->timeHorizonObst_ = timeHorizonObst;
defaultAgent_->velocity_ = velocity;
}
RVOSimulator2D::~RVOSimulator2D()
{
if (defaultAgent_ != NULL) {
delete defaultAgent_;
}
for (size_t i = 0; i < agents_.size(); ++i) {
delete agents_[i];
}
for (size_t i = 0; i < obstacles_.size(); ++i) {
delete obstacles_[i];
}
delete kdTree_;
}
size_t RVOSimulator2D::addAgent(const Vector2 &position)
{
if (defaultAgent_ == NULL) {
return RVO2D_ERROR;
}
Agent2D *agent = new Agent2D();
agent->position_ = position;
agent->maxNeighbors_ = defaultAgent_->maxNeighbors_;
agent->maxSpeed_ = defaultAgent_->maxSpeed_;
agent->neighborDist_ = defaultAgent_->neighborDist_;
agent->radius_ = defaultAgent_->radius_;
agent->timeHorizon_ = defaultAgent_->timeHorizon_;
agent->timeHorizonObst_ = defaultAgent_->timeHorizonObst_;
agent->velocity_ = defaultAgent_->velocity_;
agent->id_ = agents_.size();
agents_.push_back(agent);
return agents_.size() - 1;
}
size_t RVOSimulator2D::addAgent(const Vector2 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)
{
Agent2D *agent = new Agent2D();
agent->position_ = position;
agent->maxNeighbors_ = maxNeighbors;
agent->maxSpeed_ = maxSpeed;
agent->neighborDist_ = neighborDist;
agent->radius_ = radius;
agent->timeHorizon_ = timeHorizon;
agent->timeHorizonObst_ = timeHorizonObst;
agent->velocity_ = velocity;
agent->id_ = agents_.size();
agents_.push_back(agent);
return agents_.size() - 1;
}
size_t RVOSimulator2D::addObstacle(const std::vector<Vector2> &vertices)
{
if (vertices.size() < 2) {
return RVO2D_ERROR;
}
const size_t obstacleNo = obstacles_.size();
for (size_t i = 0; i < vertices.size(); ++i) {
Obstacle2D *obstacle = new Obstacle2D();
obstacle->point_ = vertices[i];
if (i != 0) {
obstacle->prevObstacle_ = obstacles_.back();
obstacle->prevObstacle_->nextObstacle_ = obstacle;
}
if (i == vertices.size() - 1) {
obstacle->nextObstacle_ = obstacles_[obstacleNo];
obstacle->nextObstacle_->prevObstacle_ = obstacle;
}
obstacle->unitDir_ = normalize(vertices[(i == vertices.size() - 1 ? 0 : i + 1)] - vertices[i]);
if (vertices.size() == 2) {
obstacle->isConvex_ = true;
}
else {
obstacle->isConvex_ = (leftOf(vertices[(i == 0 ? vertices.size() - 1 : i - 1)], vertices[i], vertices[(i == vertices.size() - 1 ? 0 : i + 1)]) >= 0.0f);
}
obstacle->id_ = obstacles_.size();
obstacles_.push_back(obstacle);
}
return obstacleNo;
}
void RVOSimulator2D::doStep()
{
kdTree_->buildAgentTree(agents_);
for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
agents_[i]->computeNeighbors(this);
agents_[i]->computeNewVelocity(this);
}
for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
agents_[i]->update(this);
}
globalTime_ += timeStep_;
}
size_t RVOSimulator2D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
{
return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
}
size_t RVOSimulator2D::getAgentMaxNeighbors(size_t agentNo) const
{
return agents_[agentNo]->maxNeighbors_;
}
float RVOSimulator2D::getAgentMaxSpeed(size_t agentNo) const
{
return agents_[agentNo]->maxSpeed_;
}
float RVOSimulator2D::getAgentNeighborDist(size_t agentNo) const
{
return agents_[agentNo]->neighborDist_;
}
size_t RVOSimulator2D::getAgentNumAgentNeighbors(size_t agentNo) const
{
return agents_[agentNo]->agentNeighbors_.size();
}
size_t RVOSimulator2D::getAgentNumObstacleNeighbors(size_t agentNo) const
{
return agents_[agentNo]->obstacleNeighbors_.size();
}
size_t RVOSimulator2D::getAgentNumORCALines(size_t agentNo) const
{
return agents_[agentNo]->orcaLines_.size();
}
size_t RVOSimulator2D::getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const
{
return agents_[agentNo]->obstacleNeighbors_[neighborNo].second->id_;
}
const Line &RVOSimulator2D::getAgentORCALine(size_t agentNo, size_t lineNo) const
{
return agents_[agentNo]->orcaLines_[lineNo];
}
const Vector2 &RVOSimulator2D::getAgentPosition(size_t agentNo) const
{
return agents_[agentNo]->position_;
}
const Vector2 &RVOSimulator2D::getAgentPrefVelocity(size_t agentNo) const
{
return agents_[agentNo]->prefVelocity_;
}
float RVOSimulator2D::getAgentRadius(size_t agentNo) const
{
return agents_[agentNo]->radius_;
}
float RVOSimulator2D::getAgentTimeHorizon(size_t agentNo) const
{
return agents_[agentNo]->timeHorizon_;
}
float RVOSimulator2D::getAgentTimeHorizonObst(size_t agentNo) const
{
return agents_[agentNo]->timeHorizonObst_;
}
const Vector2 &RVOSimulator2D::getAgentVelocity(size_t agentNo) const
{
return agents_[agentNo]->velocity_;
}
float RVOSimulator2D::getGlobalTime() const
{
return globalTime_;
}
size_t RVOSimulator2D::getNumAgents() const
{
return agents_.size();
}
size_t RVOSimulator2D::getNumObstacleVertices() const
{
return obstacles_.size();
}
const Vector2 &RVOSimulator2D::getObstacleVertex(size_t vertexNo) const
{
return obstacles_[vertexNo]->point_;
}
size_t RVOSimulator2D::getNextObstacleVertexNo(size_t vertexNo) const
{
return obstacles_[vertexNo]->nextObstacle_->id_;
}
size_t RVOSimulator2D::getPrevObstacleVertexNo(size_t vertexNo) const
{
return obstacles_[vertexNo]->prevObstacle_->id_;
}
float RVOSimulator2D::getTimeStep() const
{
return timeStep_;
}
void RVOSimulator2D::processObstacles()
{
kdTree_->buildObstacleTree(obstacles_);
}
bool RVOSimulator2D::queryVisibility(const Vector2 &point1, const Vector2 &point2, float radius) const
{
return kdTree_->queryVisibility(point1, point2, radius);
}
void RVOSimulator2D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float timeHorizonObst, float radius, float maxSpeed, const Vector2 &velocity)
{
if (defaultAgent_ == NULL) {
defaultAgent_ = new Agent2D();
}
defaultAgent_->maxNeighbors_ = maxNeighbors;
defaultAgent_->maxSpeed_ = maxSpeed;
defaultAgent_->neighborDist_ = neighborDist;
defaultAgent_->radius_ = radius;
defaultAgent_->timeHorizon_ = timeHorizon;
defaultAgent_->timeHorizonObst_ = timeHorizonObst;
defaultAgent_->velocity_ = velocity;
}
void RVOSimulator2D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
{
agents_[agentNo]->maxNeighbors_ = maxNeighbors;
}
void RVOSimulator2D::setAgentMaxSpeed(size_t agentNo, float maxSpeed)
{
agents_[agentNo]->maxSpeed_ = maxSpeed;
}
void RVOSimulator2D::setAgentNeighborDist(size_t agentNo, float neighborDist)
{
agents_[agentNo]->neighborDist_ = neighborDist;
}
void RVOSimulator2D::setAgentPosition(size_t agentNo, const Vector2 &position)
{
agents_[agentNo]->position_ = position;
}
void RVOSimulator2D::setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity)
{
agents_[agentNo]->prefVelocity_ = prefVelocity;
}
void RVOSimulator2D::setAgentRadius(size_t agentNo, float radius)
{
agents_[agentNo]->radius_ = radius;
}
void RVOSimulator2D::setAgentTimeHorizon(size_t agentNo, float timeHorizon)
{
agents_[agentNo]->timeHorizon_ = timeHorizon;
}
void RVOSimulator2D::setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst)
{
agents_[agentNo]->timeHorizonObst_ = timeHorizonObst;
}
void RVOSimulator2D::setAgentVelocity(size_t agentNo, const Vector2 &velocity)
{
agents_[agentNo]->velocity_ = velocity;
}
void RVOSimulator2D::setTimeStep(float timeStep)
{
timeStep_ = timeStep;
}
}

592
thirdparty/rvo2/rvo2_2d/RVOSimulator2d.h vendored
View File

@ -1,592 +0,0 @@
/*
* RVOSimulator2d.h
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#ifndef RVO2D_RVO_SIMULATOR_H_
#define RVO2D_RVO_SIMULATOR_H_
/**
* \file RVOSimulator2d.h
* \brief Contains the RVOSimulator2D class.
*/
#include <cstddef>
#include <limits>
#include <vector>
#include "Vector2.h"
namespace RVO2D {
/**
* \brief Error value.
*
* A value equal to the largest unsigned integer that is returned in case
* of an error by functions in RVO2D::RVOSimulator2D.
*/
const size_t RVO2D_ERROR = std::numeric_limits<size_t>::max();
/**
* \brief Defines a directed line.
*/
class Line {
public:
/**
* \brief A point on the directed line.
*/
Vector2 point;
/**
* \brief The direction of the directed line.
*/
Vector2 direction;
};
class Agent2D;
class KdTree2D;
class Obstacle2D;
/**
* \brief Defines the simulation.
*
* The main class of the library that contains all simulation functionality.
*/
class RVOSimulator2D {
public:
/**
* \brief Constructs a simulator instance.
*/
RVOSimulator2D();
/**
* \brief Constructs a simulator instance and sets the default
* properties for any new agent that is added.
* \param timeStep The time step of the simulation.
* Must be positive.
* \param neighborDist The default maximum distance (center point
* to center point) to other agents a new agent
* takes into account in the navigation. The
* larger this number, the longer he running
* time of the simulation. If the number is too
* low, the simulation will not be safe. Must be
* non-negative.
* \param maxNeighbors The default maximum number of other agents a
* new agent takes into account in the
* navigation. The larger this number, the
* longer the running time of the simulation.
* If the number is too low, the simulation
* will not be safe.
* \param timeHorizon The default minimal amount of time for which
* a new agent's velocities that are computed
* by the simulation are safe with respect to
* other agents. The larger this number, the
* sooner an agent will respond to the presence
* of other agents, but the less freedom the
* agent has in choosing its velocities.
* Must be positive.
* \param timeHorizonObst The default minimal amount of time for which
* a new agent's velocities that are computed
* by the simulation are safe with respect to
* obstacles. The larger this number, the
* sooner an agent will respond to the presence
* of obstacles, but the less freedom the agent
* has in choosing its velocities.
* Must be positive.
* \param radius The default radius of a new agent.
* Must be non-negative.
* \param maxSpeed The default maximum speed of a new agent.
* Must be non-negative.
* \param velocity The default initial two-dimensional linear
* velocity of a new agent (optional).
*/
RVOSimulator2D(float timeStep, float neighborDist, size_t maxNeighbors,
float timeHorizon, float timeHorizonObst, float radius,
float maxSpeed, const Vector2 &velocity = Vector2());
/**
* \brief Destroys this simulator instance.
*/
~RVOSimulator2D();
/**
* \brief Adds a new agent with default properties to the
* simulation.
* \param position The two-dimensional starting position of
* this agent.
* \return The number of the agent, or RVO2D::RVO2D_ERROR when the agent
* defaults have not been set.
*/
size_t addAgent(const Vector2 &position);
/**
* \brief Adds a new agent to the simulation.
* \param position The two-dimensional starting position of
* this agent.
* \param neighborDist The maximum distance (center point to
* center point) to other agents this agent
* takes into account in the navigation. The
* larger this number, the longer the running
* time of the simulation. If the number is too
* low, the simulation will not be safe.
* Must be non-negative.
* \param maxNeighbors The maximum number of other agents this
* agent takes into account in the navigation.
* The larger this number, the longer the
* running time of the simulation. If the
* number is too low, the simulation will not
* be safe.
* \param timeHorizon The minimal amount of time for which this
* agent's velocities that are computed by the
* simulation are safe with respect to other
* agents. The larger this number, the sooner
* this agent will respond to the presence of
* other agents, but the less freedom this
* agent has in choosing its velocities.
* Must be positive.
* \param timeHorizonObst The minimal amount of time for which this
* agent's velocities that are computed by the
* simulation are safe with respect to
* obstacles. The larger this number, the
* sooner this agent will respond to the
* presence of obstacles, but the less freedom
* this agent has in choosing its velocities.
* Must be positive.
* \param radius The radius of this agent.
* Must be non-negative.
* \param maxSpeed The maximum speed of this agent.
* Must be non-negative.
* \param velocity The initial two-dimensional linear velocity
* of this agent (optional).
* \return The number of the agent.
*/
size_t addAgent(const Vector2 &position, float neighborDist,
size_t maxNeighbors, float timeHorizon,
float timeHorizonObst, float radius, float maxSpeed,
const Vector2 &velocity = Vector2());
/**
* \brief Adds a new obstacle to the simulation.
* \param vertices List of the vertices of the polygonal
* obstacle in counterclockwise order.
* \return The number of the first vertex of the obstacle,
* or RVO2D::RVO2D_ERROR when the number of vertices is less than two.
* \note To add a "negative" obstacle, e.g. a bounding polygon around
* the environment, the vertices should be listed in clockwise
* order.
*/
size_t addObstacle(const std::vector<Vector2> &vertices);
/**
* \brief Lets the simulator perform a simulation step and updates the
* two-dimensional position and two-dimensional velocity of
* each agent.
*/
void doStep();
/**
* \brief Returns the specified agent neighbor of the specified
* agent.
* \param agentNo The number of the agent whose agent
* neighbor is to be retrieved.
* \param neighborNo The number of the agent neighbor to be
* retrieved.
* \return The number of the neighboring agent.
*/
size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const;
/**
* \brief Returns the maximum neighbor count of a specified agent.
* \param agentNo The number of the agent whose maximum
* neighbor count is to be retrieved.
* \return The present maximum neighbor count of the agent.
*/
size_t getAgentMaxNeighbors(size_t agentNo) const;
/**
* \brief Returns the maximum speed of a specified agent.
* \param agentNo The number of the agent whose maximum speed
* is to be retrieved.
* \return The present maximum speed of the agent.
*/
float getAgentMaxSpeed(size_t agentNo) const;
/**
* \brief Returns the maximum neighbor distance of a specified
* agent.
* \param agentNo The number of the agent whose maximum
* neighbor distance is to be retrieved.
* \return The present maximum neighbor distance of the agent.
*/
float getAgentNeighborDist(size_t agentNo) const;
/**
* \brief Returns the count of agent neighbors taken into account to
* compute the current velocity for the specified agent.
* \param agentNo The number of the agent whose count of agent
* neighbors is to be retrieved.
* \return The count of agent neighbors taken into account to compute
* the current velocity for the specified agent.
*/
size_t getAgentNumAgentNeighbors(size_t agentNo) const;
/**
* \brief Returns the count of obstacle neighbors taken into account
* to compute the current velocity for the specified agent.
* \param agentNo The number of the agent whose count of
* obstacle neighbors is to be retrieved.
* \return The count of obstacle neighbors taken into account to
* compute the current velocity for the specified agent.
*/
size_t getAgentNumObstacleNeighbors(size_t agentNo) const;
/**
* \brief Returns the count of ORCA constraints used to compute
* the current velocity for the specified agent.
* \param agentNo The number of the agent whose count of ORCA
* constraints is to be retrieved.
* \return The count of ORCA constraints used to compute the current
* velocity for the specified agent.
*/
size_t getAgentNumORCALines(size_t agentNo) const;
/**
* \brief Returns the specified obstacle neighbor of the specified
* agent.
* \param agentNo The number of the agent whose obstacle
* neighbor is to be retrieved.
* \param neighborNo The number of the obstacle neighbor to be
* retrieved.
* \return The number of the first vertex of the neighboring obstacle
* edge.
*/
size_t getAgentObstacleNeighbor(size_t agentNo, size_t neighborNo) const;
/**
* \brief Returns the specified ORCA constraint of the specified
* agent.
* \param agentNo The number of the agent whose ORCA
* constraint is to be retrieved.
* \param lineNo The number of the ORCA constraint to be
* retrieved.
* \return A line representing the specified ORCA constraint.
* \note The halfplane to the left of the line is the region of
* permissible velocities with respect to the specified
* ORCA constraint.
*/
const Line &getAgentORCALine(size_t agentNo, size_t lineNo) const;
/**
* \brief Returns the two-dimensional position of a specified
* agent.
* \param agentNo The number of the agent whose
* two-dimensional position is to be retrieved.
* \return The present two-dimensional position of the (center of the)
* agent.
*/
const Vector2 &getAgentPosition(size_t agentNo) const;
/**
* \brief Returns the two-dimensional preferred velocity of a
* specified agent.
* \param agentNo The number of the agent whose
* two-dimensional preferred velocity is to be
* retrieved.
* \return The present two-dimensional preferred velocity of the agent.
*/
const Vector2 &getAgentPrefVelocity(size_t agentNo) const;
/**
* \brief Returns the radius of a specified agent.
* \param agentNo The number of the agent whose radius is to
* be retrieved.
* \return The present radius of the agent.
*/
float getAgentRadius(size_t agentNo) const;
/**
* \brief Returns the time horizon of a specified agent.
* \param agentNo The number of the agent whose time horizon
* is to be retrieved.
* \return The present time horizon of the agent.
*/
float getAgentTimeHorizon(size_t agentNo) const;
/**
* \brief Returns the time horizon with respect to obstacles of a
* specified agent.
* \param agentNo The number of the agent whose time horizon
* with respect to obstacles is to be
* retrieved.
* \return The present time horizon with respect to obstacles of the
* agent.
*/
float getAgentTimeHorizonObst(size_t agentNo) const;
/**
* \brief Returns the two-dimensional linear velocity of a
* specified agent.
* \param agentNo The number of the agent whose
* two-dimensional linear velocity is to be
* retrieved.
* \return The present two-dimensional linear velocity of the agent.
*/
const Vector2 &getAgentVelocity(size_t agentNo) const;
/**
* \brief Returns the global time of the simulation.
* \return The present global time of the simulation (zero initially).
*/
float getGlobalTime() const;
/**
* \brief Returns the count of agents in the simulation.
* \return The count of agents in the simulation.
*/
size_t getNumAgents() const;
/**
* \brief Returns the count of obstacle vertices in the simulation.
* \return The count of obstacle vertices in the simulation.
*/
size_t getNumObstacleVertices() const;
/**
* \brief Returns the two-dimensional position of a specified obstacle
* vertex.
* \param vertexNo The number of the obstacle vertex to be
* retrieved.
* \return The two-dimensional position of the specified obstacle
* vertex.
*/
const Vector2 &getObstacleVertex(size_t vertexNo) const;
/**
* \brief Returns the number of the obstacle vertex succeeding the
* specified obstacle vertex in its polygon.
* \param vertexNo The number of the obstacle vertex whose
* successor is to be retrieved.
* \return The number of the obstacle vertex succeeding the specified
* obstacle vertex in its polygon.
*/
size_t getNextObstacleVertexNo(size_t vertexNo) const;
/**
* \brief Returns the number of the obstacle vertex preceding the
* specified obstacle vertex in its polygon.
* \param vertexNo The number of the obstacle vertex whose
* predecessor is to be retrieved.
* \return The number of the obstacle vertex preceding the specified
* obstacle vertex in its polygon.
*/
size_t getPrevObstacleVertexNo(size_t vertexNo) const;
/**
* \brief Returns the time step of the simulation.
* \return The present time step of the simulation.
*/
float getTimeStep() const;
/**
* \brief Processes the obstacles that have been added so that they
* are accounted for in the simulation.
* \note Obstacles added to the simulation after this function has
* been called are not accounted for in the simulation.
*/
void processObstacles();
/**
* \brief Performs a visibility query between the two specified
* points with respect to the obstacles
* \param point1 The first point of the query.
* \param point2 The second point of the query.
* \param radius The minimal distance between the line
* connecting the two points and the obstacles
* in order for the points to be mutually
* visible (optional). Must be non-negative.
* \return A boolean specifying whether the two points are mutually
* visible. Returns true when the obstacles have not been
* processed.
*/
bool queryVisibility(const Vector2 &point1, const Vector2 &point2,
float radius = 0.0f) const;
/**
* \brief Sets the default properties for any new agent that is
* added.
* \param neighborDist The default maximum distance (center point
* to center point) to other agents a new agent
* takes into account in the navigation. The
* larger this number, the longer he running
* time of the simulation. If the number is too
* low, the simulation will not be safe.
* Must be non-negative.
* \param maxNeighbors The default maximum number of other agents a
* new agent takes into account in the
* navigation. The larger this number, the
* longer the running time of the simulation.
* If the number is too low, the simulation
* will not be safe.
* \param timeHorizon The default minimal amount of time for which
* a new agent's velocities that are computed
* by the simulation are safe with respect to
* other agents. The larger this number, the
* sooner an agent will respond to the presence
* of other agents, but the less freedom the
* agent has in choosing its velocities.
* Must be positive.
* \param timeHorizonObst The default minimal amount of time for which
* a new agent's velocities that are computed
* by the simulation are safe with respect to
* obstacles. The larger this number, the
* sooner an agent will respond to the presence
* of obstacles, but the less freedom the agent
* has in choosing its velocities.
* Must be positive.
* \param radius The default radius of a new agent.
* Must be non-negative.
* \param maxSpeed The default maximum speed of a new agent.
* Must be non-negative.
* \param velocity The default initial two-dimensional linear
* velocity of a new agent (optional).
*/
void setAgentDefaults(float neighborDist, size_t maxNeighbors,
float timeHorizon, float timeHorizonObst,
float radius, float maxSpeed,
const Vector2 &velocity = Vector2());
/**
* \brief Sets the maximum neighbor count of a specified agent.
* \param agentNo The number of the agent whose maximum
* neighbor count is to be modified.
* \param maxNeighbors The replacement maximum neighbor count.
*/
void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors);
/**
* \brief Sets the maximum speed of a specified agent.
* \param agentNo The number of the agent whose maximum speed
* is to be modified.
* \param maxSpeed The replacement maximum speed. Must be
* non-negative.
*/
void setAgentMaxSpeed(size_t agentNo, float maxSpeed);
/**
* \brief Sets the maximum neighbor distance of a specified agent.
* \param agentNo The number of the agent whose maximum
* neighbor distance is to be modified.
* \param neighborDist The replacement maximum neighbor distance.
* Must be non-negative.
*/
void setAgentNeighborDist(size_t agentNo, float neighborDist);
/**
* \brief Sets the two-dimensional position of a specified agent.
* \param agentNo The number of the agent whose
* two-dimensional position is to be modified.
* \param position The replacement of the two-dimensional
* position.
*/
void setAgentPosition(size_t agentNo, const Vector2 &position);
/**
* \brief Sets the two-dimensional preferred velocity of a
* specified agent.
* \param agentNo The number of the agent whose
* two-dimensional preferred velocity is to be
* modified.
* \param prefVelocity The replacement of the two-dimensional
* preferred velocity.
*/
void setAgentPrefVelocity(size_t agentNo, const Vector2 &prefVelocity);
/**
* \brief Sets the radius of a specified agent.
* \param agentNo The number of the agent whose radius is to
* be modified.
* \param radius The replacement radius.
* Must be non-negative.
*/
void setAgentRadius(size_t agentNo, float radius);
/**
* \brief Sets the time horizon of a specified agent with respect
* to other agents.
* \param agentNo The number of the agent whose time horizon
* is to be modified.
* \param timeHorizon The replacement time horizon with respect
* to other agents. Must be positive.
*/
void setAgentTimeHorizon(size_t agentNo, float timeHorizon);
/**
* \brief Sets the time horizon of a specified agent with respect
* to obstacles.
* \param agentNo The number of the agent whose time horizon
* with respect to obstacles is to be modified.
* \param timeHorizonObst The replacement time horizon with respect to
* obstacles. Must be positive.
*/
void setAgentTimeHorizonObst(size_t agentNo, float timeHorizonObst);
/**
* \brief Sets the two-dimensional linear velocity of a specified
* agent.
* \param agentNo The number of the agent whose
* two-dimensional linear velocity is to be
* modified.
* \param velocity The replacement two-dimensional linear
* velocity.
*/
void setAgentVelocity(size_t agentNo, const Vector2 &velocity);
/**
* \brief Sets the time step of the simulation.
* \param timeStep The time step of the simulation.
* Must be positive.
*/
void setTimeStep(float timeStep);
public:
std::vector<Agent2D *> agents_;
Agent2D *defaultAgent_;
float globalTime_;
KdTree2D *kdTree_;
std::vector<Obstacle2D *> obstacles_;
float timeStep_;
friend class Agent2D;
friend class KdTree2D;
friend class Obstacle2D;
};
}
#endif /* RVO2D_RVO_SIMULATOR_H_ */

346
thirdparty/rvo2/rvo2_2d/Vector2.h vendored
View File

@ -1,346 +0,0 @@
/*
* Vector2.h
* RVO2 Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#ifndef RVO_VECTOR2_H_
#define RVO_VECTOR2_H_
/**
* \file Vector2.h
* \brief Contains the Vector2 class.
*/
#include <cmath>
#include <ostream>
namespace RVO2D {
/**
* \brief Defines a two-dimensional vector.
*/
class Vector2 {
public:
/**
* \brief Constructs and initializes a two-dimensional vector instance
* to (0.0, 0.0).
*/
inline Vector2() : x_(0.0f), y_(0.0f) { }
/**
* \brief Constructs and initializes a two-dimensional vector from
* the specified xy-coordinates.
* \param x The x-coordinate of the two-dimensional
* vector.
* \param y The y-coordinate of the two-dimensional
* vector.
*/
inline Vector2(float x, float y) : x_(x), y_(y) { }
inline Vector2(const Vector2 &vector)
{
x_ = vector.x();
y_ = vector.y();
}
/**
* \brief Returns the x-coordinate of this two-dimensional vector.
* \return The x-coordinate of the two-dimensional vector.
*/
inline float x() const { return x_; }
/**
* \brief Returns the y-coordinate of this two-dimensional vector.
* \return The y-coordinate of the two-dimensional vector.
*/
inline float y() const { return y_; }
/**
* \brief Computes the negation of this two-dimensional vector.
* \return The negation of this two-dimensional vector.
*/
inline Vector2 operator-() const
{
return Vector2(-x_, -y_);
}
/**
* \brief Computes the dot product of this two-dimensional vector with
* the specified two-dimensional vector.
* \param vector The two-dimensional vector with which the
* dot product should be computed.
* \return The dot product of this two-dimensional vector with a
* specified two-dimensional vector.
*/
inline float operator*(const Vector2 &vector) const
{
return x_ * vector.x() + y_ * vector.y();
}
/**
* \brief Computes the scalar multiplication of this
* two-dimensional vector with the specified scalar value.
* \param s The scalar value with which the scalar
* multiplication should be computed.
* \return The scalar multiplication of this two-dimensional vector
* with a specified scalar value.
*/
inline Vector2 operator*(float s) const
{
return Vector2(x_ * s, y_ * s);
}
/**
* \brief Computes the scalar division of this two-dimensional vector
* with the specified scalar value.
* \param s The scalar value with which the scalar
* division should be computed.
* \return The scalar division of this two-dimensional vector with a
* specified scalar value.
*/
inline Vector2 operator/(float s) const
{
const float invS = 1.0f / s;
return Vector2(x_ * invS, y_ * invS);
}
/**
* \brief Computes the vector sum of this two-dimensional vector with
* the specified two-dimensional vector.
* \param vector The two-dimensional vector with which the
* vector sum should be computed.
* \return The vector sum of this two-dimensional vector with a
* specified two-dimensional vector.
*/
inline Vector2 operator+(const Vector2 &vector) const
{
return Vector2(x_ + vector.x(), y_ + vector.y());
}
/**
* \brief Computes the vector difference of this two-dimensional
* vector with the specified two-dimensional vector.
* \param vector The two-dimensional vector with which the
* vector difference should be computed.
* \return The vector difference of this two-dimensional vector with a
* specified two-dimensional vector.
*/
inline Vector2 operator-(const Vector2 &vector) const
{
return Vector2(x_ - vector.x(), y_ - vector.y());
}
/**
* \brief Tests this two-dimensional vector for equality with the
* specified two-dimensional vector.
* \param vector The two-dimensional vector with which to
* test for equality.
* \return True if the two-dimensional vectors are equal.
*/
inline bool operator==(const Vector2 &vector) const
{
return x_ == vector.x() && y_ == vector.y();
}
/**
* \brief Tests this two-dimensional vector for inequality with the
* specified two-dimensional vector.
* \param vector The two-dimensional vector with which to
* test for inequality.
* \return True if the two-dimensional vectors are not equal.
*/
inline bool operator!=(const Vector2 &vector) const
{
return x_ != vector.x() || y_ != vector.y();
}
/**
* \brief Sets the value of this two-dimensional vector to the scalar
* multiplication of itself with the specified scalar value.
* \param s The scalar value with which the scalar
* multiplication should be computed.
* \return A reference to this two-dimensional vector.
*/
inline Vector2 &operator*=(float s)
{
x_ *= s;
y_ *= s;
return *this;
}
/**
* \brief Sets the value of this two-dimensional vector to the scalar
* division of itself with the specified scalar value.
* \param s The scalar value with which the scalar
* division should be computed.
* \return A reference to this two-dimensional vector.
*/
inline Vector2 &operator/=(float s)
{
const float invS = 1.0f / s;
x_ *= invS;
y_ *= invS;
return *this;
}
/**
* \brief Sets the value of this two-dimensional vector to the vector
* sum of itself with the specified two-dimensional vector.
* \param vector The two-dimensional vector with which the
* vector sum should be computed.
* \return A reference to this two-dimensional vector.
*/
inline Vector2 &operator+=(const Vector2 &vector)
{
x_ += vector.x();
y_ += vector.y();
return *this;
}
/**
* \brief Sets the value of this two-dimensional vector to the vector
* difference of itself with the specified two-dimensional
* vector.
* \param vector The two-dimensional vector with which the
* vector difference should be computed.
* \return A reference to this two-dimensional vector.
*/
inline Vector2 &operator-=(const Vector2 &vector)
{
x_ -= vector.x();
y_ -= vector.y();
return *this;
}
inline Vector2 &operator=(const Vector2 &vector)
{
x_ = vector.x();
y_ = vector.y();
return *this;
}
private:
float x_;
float y_;
};
/**
* \relates Vector2
* \brief Computes the scalar multiplication of the specified
* two-dimensional vector with the specified scalar value.
* \param s The scalar value with which the scalar
* multiplication should be computed.
* \param vector The two-dimensional vector with which the scalar
* multiplication should be computed.
* \return The scalar multiplication of the two-dimensional vector with the
* scalar value.
*/
inline Vector2 operator*(float s, const Vector2 &vector)
{
return Vector2(s * vector.x(), s * vector.y());
}
/**
* \relates Vector2
* \brief Inserts the specified two-dimensional vector into the specified
* output stream.
* \param os The output stream into which the two-dimensional
* vector should be inserted.
* \param vector The two-dimensional vector which to insert into
* the output stream.
* \return A reference to the output stream.
*/
inline std::ostream &operator<<(std::ostream &os, const Vector2 &vector)
{
os << "(" << vector.x() << "," << vector.y() << ")";
return os;
}
/**
* \relates Vector2
* \brief Computes the length of a specified two-dimensional vector.
* \param vector The two-dimensional vector whose length is to be
* computed.
* \return The length of the two-dimensional vector.
*/
inline float abs(const Vector2 &vector)
{
return std::sqrt(vector * vector);
}
/**
* \relates Vector2
* \brief Computes the squared length of a specified two-dimensional
* vector.
* \param vector The two-dimensional vector whose squared length
* is to be computed.
* \return The squared length of the two-dimensional vector.
*/
inline float absSq(const Vector2 &vector)
{
return vector * vector;
}
/**
* \relates Vector2
* \brief Computes the determinant of a two-dimensional square matrix with
* rows consisting of the specified two-dimensional vectors.
* \param vector1 The top row of the two-dimensional square
* matrix.
* \param vector2 The bottom row of the two-dimensional square
* matrix.
* \return The determinant of the two-dimensional square matrix.
*/
inline float det(const Vector2 &vector1, const Vector2 &vector2)
{
return vector1.x() * vector2.y() - vector1.y() * vector2.x();
}
/**
* \relates Vector2
* \brief Computes the normalization of the specified two-dimensional
* vector.
* \param vector The two-dimensional vector whose normalization
* is to be computed.
* \return The normalization of the two-dimensional vector.
*/
inline Vector2 normalize(const Vector2 &vector)
{
return vector / abs(vector);
}
}
#endif /* RVO_VECTOR2_H_ */

449
thirdparty/rvo2/rvo2_3d/Agent3d.cpp vendored
View File

@ -1,449 +0,0 @@
/*
* Agent.cpp
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* https://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <https://gamma.cs.unc.edu/RVO2/>
*/
#include "Agent3d.h"
#include <cmath>
#include <algorithm>
#include "Definitions.h"
#include "KdTree3d.h"
namespace RVO3D {
/**
* \brief A sufficiently small positive number.
*/
const float RVO3D_EPSILON = 0.00001f;
/**
* \brief Defines a directed line.
*/
class Line3D {
public:
/**
* \brief The direction of the directed line.
*/
Vector3 direction;
/**
* \brief A point on the directed line.
*/
Vector3 point;
};
/**
* \brief Solves a one-dimensional linear program on a specified line subject to linear constraints defined by planes and a spherical constraint.
* \param planes Planes defining the linear constraints.
* \param planeNo The plane on which the line lies.
* \param line The line on which the 1-d linear program is solved
* \param radius The radius of the spherical constraint.
* \param optVelocity The optimization velocity.
* \param directionOpt True if the direction should be optimized.
* \param result A reference to the result of the linear program.
* \return True if successful.
*/
bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line3D &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
/**
* \brief Solves a two-dimensional linear program on a specified plane subject to linear constraints defined by planes and a spherical constraint.
* \param planes Planes defining the linear constraints.
* \param planeNo The plane on which the 2-d linear program is solved
* \param radius The radius of the spherical constraint.
* \param optVelocity The optimization velocity.
* \param directionOpt True if the direction should be optimized.
* \param result A reference to the result of the linear program.
* \return True if successful.
*/
bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
/**
* \brief Solves a three-dimensional linear program subject to linear constraints defined by planes and a spherical constraint.
* \param planes Planes defining the linear constraints.
* \param radius The radius of the spherical constraint.
* \param optVelocity The optimization velocity.
* \param directionOpt True if the direction should be optimized.
* \param result A reference to the result of the linear program.
* \return The number of the plane it fails on, and the number of planes if successful.
*/
size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result);
/**
* \brief Solves a four-dimensional linear program subject to linear constraints defined by planes and a spherical constraint.
* \param planes Planes defining the linear constraints.
* \param beginPlane The plane on which the 3-d linear program failed.
* \param radius The radius of the spherical constraint.
* \param result A reference to the result of the linear program.
*/
void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result);
Agent3D::Agent3D() : id_(0), maxNeighbors_(0), maxSpeed_(0.0f), neighborDist_(0.0f), radius_(0.0f), timeHorizon_(0.0f) { }
void Agent3D::computeNeighbors(RVOSimulator3D *sim_)
{
agentNeighbors_.clear();
if (maxNeighbors_ > 0) {
sim_->kdTree_->computeAgentNeighbors(this, neighborDist_ * neighborDist_);
}
}
void Agent3D::computeNewVelocity(RVOSimulator3D *sim_)
{
orcaPlanes_.clear();
const float invTimeHorizon = 1.0f / timeHorizon_;
/* Create agent ORCA planes. */
for (size_t i = 0; i < agentNeighbors_.size(); ++i) {
const Agent3D *const other = agentNeighbors_[i].second;
//const float timeHorizon_mod = (avoidance_priority_ - other->avoidance_priority_ + 1.0f) * 0.5f;
//const float invTimeHorizon = (1.0f / timeHorizon_) * timeHorizon_mod;
const Vector3 relativePosition = other->position_ - position_;
const Vector3 relativeVelocity = velocity_ - other->velocity_;
const float distSq = absSq(relativePosition);
const float combinedRadius = radius_ + other->radius_;
const float combinedRadiusSq = sqr(combinedRadius);
Plane plane;
Vector3 u;
if (distSq > combinedRadiusSq) {
/* No collision. */
const Vector3 w = relativeVelocity - invTimeHorizon * relativePosition;
/* Vector from cutoff center to relative velocity. */
const float wLengthSq = absSq(w);
const float dotProduct = w * relativePosition;
if (dotProduct < 0.0f && sqr(dotProduct) > combinedRadiusSq * wLengthSq) {
/* Project on cut-off circle. */
const float wLength = std::sqrt(wLengthSq);
const Vector3 unitW = w / wLength;
plane.normal = unitW;
u = (combinedRadius * invTimeHorizon - wLength) * unitW;
}
else {
/* Project on cone. */
const float a = distSq;
const float b = relativePosition * relativeVelocity;
const float c = absSq(relativeVelocity) - absSq(cross(relativePosition, relativeVelocity)) / (distSq - combinedRadiusSq);
const float t = (b + std::sqrt(sqr(b) - a * c)) / a;
const Vector3 w = relativeVelocity - t * relativePosition;
const float wLength = abs(w);
const Vector3 unitW = w / wLength;
plane.normal = unitW;
u = (combinedRadius * t - wLength) * unitW;
}
}
else {
/* Collision. */
const float invTimeStep = 1.0f / sim_->timeStep_;
const Vector3 w = relativeVelocity - invTimeStep * relativePosition;
const float wLength = abs(w);
const Vector3 unitW = w / wLength;
plane.normal = unitW;
u = (combinedRadius * invTimeStep - wLength) * unitW;
}
plane.point = velocity_ + 0.5f * u;
orcaPlanes_.push_back(plane);
}
const size_t planeFail = linearProgram3(orcaPlanes_, maxSpeed_, prefVelocity_, false, newVelocity_);
if (planeFail < orcaPlanes_.size()) {
linearProgram4(orcaPlanes_, planeFail, maxSpeed_, newVelocity_);
}
}
void Agent3D::insertAgentNeighbor(const Agent3D *agent, float &rangeSq)
{
// no point processing same agent
if (this == agent) {
return;
}
// ignore other agent if layers/mask bitmasks have no matching bit
if ((avoidance_mask_ & agent->avoidance_layers_) == 0) {
return;
}
if (avoidance_priority_ > agent->avoidance_priority_) {
return;
}
const float distSq = absSq(position_ - agent->position_);
if (distSq < rangeSq) {
if (agentNeighbors_.size() < maxNeighbors_) {
agentNeighbors_.push_back(std::make_pair(distSq, agent));
}
size_t i = agentNeighbors_.size() - 1;
while (i != 0 && distSq < agentNeighbors_[i - 1].first) {
agentNeighbors_[i] = agentNeighbors_[i - 1];
--i;
}
agentNeighbors_[i] = std::make_pair(distSq, agent);
if (agentNeighbors_.size() == maxNeighbors_) {
rangeSq = agentNeighbors_.back().first;
}
}
}
void Agent3D::update(RVOSimulator3D *sim_)
{
velocity_ = newVelocity_;
position_ += velocity_ * sim_->timeStep_;
}
bool linearProgram1(const std::vector<Plane> &planes, size_t planeNo, const Line3D &line, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
{
const float dotProduct = line.point * line.direction;
const float discriminant = sqr(dotProduct) + sqr(radius) - absSq(line.point);
if (discriminant < 0.0f) {
/* Max speed sphere fully invalidates line. */
return false;
}
const float sqrtDiscriminant = std::sqrt(discriminant);
float tLeft = -dotProduct - sqrtDiscriminant;
float tRight = -dotProduct + sqrtDiscriminant;
for (size_t i = 0; i < planeNo; ++i) {
const float numerator = (planes[i].point - line.point) * planes[i].normal;
const float denominator = line.direction * planes[i].normal;
if (sqr(denominator) <= RVO3D_EPSILON) {
/* Lines3D line is (almost) parallel to plane i. */
if (numerator > 0.0f) {
return false;
}
else {
continue;
}
}
const float t = numerator / denominator;
if (denominator >= 0.0f) {
/* Plane i bounds line on the left. */
tLeft = std::max(tLeft, t);
}
else {
/* Plane i bounds line on the right. */
tRight = std::min(tRight, t);
}
if (tLeft > tRight) {
return false;
}
}
if (directionOpt) {
/* Optimize direction. */
if (optVelocity * line.direction > 0.0f) {
/* Take right extreme. */
result = line.point + tRight * line.direction;
}
else {
/* Take left extreme. */
result = line.point + tLeft * line.direction;
}
}
else {
/* Optimize closest point. */
const float t = line.direction * (optVelocity - line.point);
if (t < tLeft) {
result = line.point + tLeft * line.direction;
}
else if (t > tRight) {
result = line.point + tRight * line.direction;
}
else {
result = line.point + t * line.direction;
}
}
return true;
}
bool linearProgram2(const std::vector<Plane> &planes, size_t planeNo, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
{
const float planeDist = planes[planeNo].point * planes[planeNo].normal;
const float planeDistSq = sqr(planeDist);
const float radiusSq = sqr(radius);
if (planeDistSq > radiusSq) {
/* Max speed sphere fully invalidates plane planeNo. */
return false;
}
const float planeRadiusSq = radiusSq - planeDistSq;
const Vector3 planeCenter = planeDist * planes[planeNo].normal;
if (directionOpt) {
/* Project direction optVelocity on plane planeNo. */
const Vector3 planeOptVelocity = optVelocity - (optVelocity * planes[planeNo].normal) * planes[planeNo].normal;
const float planeOptVelocityLengthSq = absSq(planeOptVelocity);
if (planeOptVelocityLengthSq <= RVO3D_EPSILON) {
result = planeCenter;
}
else {
result = planeCenter + std::sqrt(planeRadiusSq / planeOptVelocityLengthSq) * planeOptVelocity;
}
}
else {
/* Project point optVelocity on plane planeNo. */
result = optVelocity + ((planes[planeNo].point - optVelocity) * planes[planeNo].normal) * planes[planeNo].normal;
/* If outside planeCircle, project on planeCircle. */
if (absSq(result) > radiusSq) {
const Vector3 planeResult = result - planeCenter;
const float planeResultLengthSq = absSq(planeResult);
result = planeCenter + std::sqrt(planeRadiusSq / planeResultLengthSq) * planeResult;
}
}
for (size_t i = 0; i < planeNo; ++i) {
if (planes[i].normal * (planes[i].point - result) > 0.0f) {
/* Result does not satisfy constraint i. Compute new optimal result. */
/* Compute intersection line of plane i and plane planeNo. */
Vector3 crossProduct = cross(planes[i].normal, planes[planeNo].normal);
if (absSq(crossProduct) <= RVO3D_EPSILON) {
/* Planes planeNo and i are (almost) parallel, and plane i fully invalidates plane planeNo. */
return false;
}
Line3D line;
line.direction = normalize(crossProduct);
const Vector3 lineNormal = cross(line.direction, planes[planeNo].normal);
line.point = planes[planeNo].point + (((planes[i].point - planes[planeNo].point) * planes[i].normal) / (lineNormal * planes[i].normal)) * lineNormal;
if (!linearProgram1(planes, i, line, radius, optVelocity, directionOpt, result)) {
return false;
}
}
}
return true;
}
size_t linearProgram3(const std::vector<Plane> &planes, float radius, const Vector3 &optVelocity, bool directionOpt, Vector3 &result)
{
if (directionOpt) {
/* Optimize direction. Note that the optimization velocity is of unit length in this case. */
result = optVelocity * radius;
}
else if (absSq(optVelocity) > sqr(radius)) {
/* Optimize closest point and outside circle. */
result = normalize(optVelocity) * radius;
}
else {
/* Optimize closest point and inside circle. */
result = optVelocity;
}
for (size_t i = 0; i < planes.size(); ++i) {
if (planes[i].normal * (planes[i].point - result) > 0.0f) {
/* Result does not satisfy constraint i. Compute new optimal result. */
const Vector3 tempResult = result;
if (!linearProgram2(planes, i, radius, optVelocity, directionOpt, result)) {
result = tempResult;
return i;
}
}
}
return planes.size();
}
void linearProgram4(const std::vector<Plane> &planes, size_t beginPlane, float radius, Vector3 &result)
{
float distance = 0.0f;
for (size_t i = beginPlane; i < planes.size(); ++i) {
if (planes[i].normal * (planes[i].point - result) > distance) {
/* Result does not satisfy constraint of plane i. */
std::vector<Plane> projPlanes;
for (size_t j = 0; j < i; ++j) {
Plane plane;
const Vector3 crossProduct = cross(planes[j].normal, planes[i].normal);
if (absSq(crossProduct) <= RVO3D_EPSILON) {
/* Plane i and plane j are (almost) parallel. */
if (planes[i].normal * planes[j].normal > 0.0f) {
/* Plane i and plane j point in the same direction. */
continue;
}
else {
/* Plane i and plane j point in opposite direction. */
plane.point = 0.5f * (planes[i].point + planes[j].point);
}
}
else {
/* Plane.point is point on line of intersection between plane i and plane j. */
const Vector3 lineNormal = cross(crossProduct, planes[i].normal);
plane.point = planes[i].point + (((planes[j].point - planes[i].point) * planes[j].normal) / (lineNormal * planes[j].normal)) * lineNormal;
}
plane.normal = normalize(planes[j].normal - planes[i].normal);
projPlanes.push_back(plane);
}
const Vector3 tempResult = result;
if (linearProgram3(projPlanes, radius, planes[i].normal, true, result) < projPlanes.size()) {
/* This should in principle not happen. The result is by definition already in the feasible region of this linear program. If it fails, it is due to small floating point error, and the current result is kept. */
result = tempResult;
}
distance = planes[i].normal * (planes[i].point - result);
}
}
}
}

106
thirdparty/rvo2/rvo2_3d/Agent3d.h vendored
View File

@ -1,106 +0,0 @@
/*
* Agent.h
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file Agent.h
* \brief Contains the Agent class.
*/
#ifndef RVO3D_AGENT_H_
#define RVO3D_AGENT_H_
#include <cstddef>
#include <cstdint>
#include <utility>
#include <vector>
#include "RVOSimulator3d.h"
#include "Vector3.h"
namespace RVO3D {
/**
* \brief Defines an agent in the simulation.
*/
class Agent3D {
public:
/**
* \brief Constructs an agent instance.
* \param sim The simulator instance.
*/
explicit Agent3D();
/**
* \brief Computes the neighbors of this agent.
*/
void computeNeighbors(RVOSimulator3D *sim_);
/**
* \brief Computes the new velocity of this agent.
*/
void computeNewVelocity(RVOSimulator3D *sim_);
/**
* \brief Inserts an agent neighbor into the set of neighbors of this agent.
* \param agent A pointer to the agent to be inserted.
* \param rangeSq The squared range around this agent.
*/
void insertAgentNeighbor(const Agent3D *agent, float &rangeSq);
/**
* \brief Updates the three-dimensional position and three-dimensional velocity of this agent.
*/
void update(RVOSimulator3D *sim_);
Vector3 newVelocity_;
Vector3 position_;
Vector3 prefVelocity_;
Vector3 velocity_;
RVOSimulator3D *sim_;
size_t id_;
size_t maxNeighbors_;
float maxSpeed_;
float neighborDist_;
float radius_;
float timeHorizon_;
float timeHorizonObst_;
std::vector<std::pair<float, const Agent3D *> > agentNeighbors_;
std::vector<Plane> orcaPlanes_;
float height_ = 1.0;
uint32_t avoidance_layers_ = 1;
uint32_t avoidance_mask_ = 1;
float avoidance_priority_ = 1.0;
friend class KdTree3D;
friend class RVOSimulator3D;
};
}
#endif /* RVO3D_AGENT_H_ */

53
thirdparty/rvo2/rvo2_3d/Definitions.h vendored
View File

@ -1,53 +0,0 @@
/*
* Definitions.h
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* https://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <https://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file Definitions.h
* \brief Contains functions and constants used in multiple classes.
*/
#ifndef RVO3D_DEFINITIONS_H_
#define RVO3D_DEFINITIONS_H_
namespace RVO3D {
/**
* \brief Computes the square of a float.
* \param scalar The float to be squared.
* \return The square of the float.
*/
inline float sqr(float scalar)
{
return scalar * scalar;
}
}
#endif /* RVO3D_DEFINITIONS_H_ */

161
thirdparty/rvo2/rvo2_3d/KdTree3d.cpp vendored
View File

@ -1,161 +0,0 @@
/*
* KdTree.cpp
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* https://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <https://gamma.cs.unc.edu/RVO2/>
*/
#include "KdTree3d.h"
#include <algorithm>
#include "Agent3d.h"
#include "Definitions.h"
#include "RVOSimulator3d.h"
namespace RVO3D {
const size_t RVO3D_MAX_LEAF_SIZE = 10;
KdTree3D::KdTree3D(RVOSimulator3D *sim) : sim_(sim) { }
void KdTree3D::buildAgentTree(std::vector<Agent3D *> agents)
{
agents_.swap(agents);
if (!agents_.empty()) {
agentTree_.resize(2 * agents_.size() - 1);
buildAgentTreeRecursive(0, agents_.size(), 0);
}
}
void KdTree3D::buildAgentTreeRecursive(size_t begin, size_t end, size_t node)
{
agentTree_[node].begin = begin;
agentTree_[node].end = end;
agentTree_[node].minCoord = agents_[begin]->position_;
agentTree_[node].maxCoord = agents_[begin]->position_;
for (size_t i = begin + 1; i < end; ++i) {
agentTree_[node].maxCoord[0] = std::max(agentTree_[node].maxCoord[0], agents_[i]->position_.x());
agentTree_[node].minCoord[0] = std::min(agentTree_[node].minCoord[0], agents_[i]->position_.x());
agentTree_[node].maxCoord[1] = std::max(agentTree_[node].maxCoord[1], agents_[i]->position_.y());
agentTree_[node].minCoord[1] = std::min(agentTree_[node].minCoord[1], agents_[i]->position_.y());
agentTree_[node].maxCoord[2] = std::max(agentTree_[node].maxCoord[2], agents_[i]->position_.z());
agentTree_[node].minCoord[2] = std::min(agentTree_[node].minCoord[2], agents_[i]->position_.z());
}
if (end - begin > RVO3D_MAX_LEAF_SIZE) {
/* No leaf node. */
size_t coord;
if (agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] && agentTree_[node].maxCoord[0] - agentTree_[node].minCoord[0] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
coord = 0;
}
else if (agentTree_[node].maxCoord[1] - agentTree_[node].minCoord[1] > agentTree_[node].maxCoord[2] - agentTree_[node].minCoord[2]) {
coord = 1;
}
else {
coord = 2;
}
const float splitValue = 0.5f * (agentTree_[node].maxCoord[coord] + agentTree_[node].minCoord[coord]);
size_t left = begin;
size_t right = end;
while (left < right) {
while (left < right && agents_[left]->position_[coord] < splitValue) {
++left;
}
while (right > left && agents_[right - 1]->position_[coord] >= splitValue) {
--right;
}
if (left < right) {
std::swap(agents_[left], agents_[right - 1]);
++left;
--right;
}
}
size_t leftSize = left - begin;
if (leftSize == 0) {
++leftSize;
++left;
++right;
}
agentTree_[node].left = node + 1;
agentTree_[node].right = node + 2 * leftSize;
buildAgentTreeRecursive(begin, left, agentTree_[node].left);
buildAgentTreeRecursive(left, end, agentTree_[node].right);
}
}
void KdTree3D::computeAgentNeighbors(Agent3D *agent, float rangeSq) const
{
queryAgentTreeRecursive(agent, rangeSq, 0);
}
void KdTree3D::queryAgentTreeRecursive(Agent3D *agent, float &rangeSq, size_t node) const
{
if (agentTree_[node].end - agentTree_[node].begin <= RVO3D_MAX_LEAF_SIZE) {
for (size_t i = agentTree_[node].begin; i < agentTree_[node].end; ++i) {
agent->insertAgentNeighbor(agents_[i], rangeSq);
}
}
else {
const float distSqLeft = sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[0] - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].left].maxCoord[0])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[1] - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].left].maxCoord[1])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].left].minCoord[2] - agent->position_.z())) + sqr(std::max(0.0f, agent->position_.z() - agentTree_[agentTree_[node].left].maxCoord[2]));
const float distSqRight = sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[0] - agent->position_.x())) + sqr(std::max(0.0f, agent->position_.x() - agentTree_[agentTree_[node].right].maxCoord[0])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[1] - agent->position_.y())) + sqr(std::max(0.0f, agent->position_.y() - agentTree_[agentTree_[node].right].maxCoord[1])) + sqr(std::max(0.0f, agentTree_[agentTree_[node].right].minCoord[2] - agent->position_.z())) + sqr(std::max(0.0f, agent->position_.z() - agentTree_[agentTree_[node].right].maxCoord[2]));
if (distSqLeft < distSqRight) {
if (distSqLeft < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
if (distSqRight < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
}
}
}
else {
if (distSqRight < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].right);
if (distSqLeft < rangeSq) {
queryAgentTreeRecursive(agent, rangeSq, agentTree_[node].left);
}
}
}
}
}
}

120
thirdparty/rvo2/rvo2_3d/KdTree3d.h vendored
View File

@ -1,120 +0,0 @@
/*
* KdTree.h
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* https://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <https://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file KdTree.h
* \brief Contains the KdTree class.
*/
#ifndef RVO3D_KD_TREE_H_
#define RVO3D_KD_TREE_H_
#include <cstddef>
#include <vector>
#include "Vector3.h"
namespace RVO3D {
class Agent3D;
class RVOSimulator3D;
/**
* \brief Defines <i>k</i>d-trees for agents in the simulation.
*/
class KdTree3D {
public:
/**
* \brief Defines an agent <i>k</i>d-tree node.
*/
class AgentTreeNode3D {
public:
/**
* \brief The beginning node number.
*/
size_t begin;
/**
* \brief The ending node number.
*/
size_t end;
/**
* \brief The left node number.
*/
size_t left;
/**
* \brief The right node number.
*/
size_t right;
/**
* \brief The maximum coordinates.
*/
Vector3 maxCoord;
/**
* \brief The minimum coordinates.
*/
Vector3 minCoord;
};
/**
* \brief Constructs a <i>k</i>d-tree instance.
* \param sim The simulator instance.
*/
explicit KdTree3D(RVOSimulator3D *sim);
/**
* \brief Builds an agent <i>k</i>d-tree.
*/
void buildAgentTree(std::vector<Agent3D *> agents);
void buildAgentTreeRecursive(size_t begin, size_t end, size_t node);
/**
* \brief Computes the agent neighbors of the specified agent.
* \param agent A pointer to the agent for which agent neighbors are to be computed.
* \param rangeSq The squared range around the agent.
*/
void computeAgentNeighbors(Agent3D *agent, float rangeSq) const;
void queryAgentTreeRecursive(Agent3D *agent, float &rangeSq, size_t node) const;
std::vector<Agent3D *> agents_;
std::vector<AgentTreeNode3D> agentTree_;
RVOSimulator3D *sim_;
friend class Agent3D;
friend class RVOSimulator3D;
};
}
#endif /* RVO3D_KD_TREE_H_ */

274
thirdparty/rvo2/rvo2_3d/RVOSimulator3d.cpp vendored
View File

@ -1,274 +0,0 @@
/*
* RVOSimulator.cpp
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
#include "RVOSimulator3d.h"
#ifdef _OPENMP
#include <omp.h>
#endif
#include "Agent3d.h"
#include "KdTree3d.h"
namespace RVO3D {
RVOSimulator3D::RVOSimulator3D() : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(0.0f)
{
kdTree_ = new KdTree3D(this);
}
RVOSimulator3D::RVOSimulator3D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity) : defaultAgent_(NULL), kdTree_(NULL), globalTime_(0.0f), timeStep_(timeStep)
{
kdTree_ = new KdTree3D(this);
defaultAgent_ = new Agent3D();
defaultAgent_->maxNeighbors_ = maxNeighbors;
defaultAgent_->maxSpeed_ = maxSpeed;
defaultAgent_->neighborDist_ = neighborDist;
defaultAgent_->radius_ = radius;
defaultAgent_->timeHorizon_ = timeHorizon;
defaultAgent_->velocity_ = velocity;
}
RVOSimulator3D::~RVOSimulator3D()
{
if (defaultAgent_ != NULL) {
delete defaultAgent_;
}
for (size_t i = 0; i < agents_.size(); ++i) {
delete agents_[i];
}
if (kdTree_ != NULL) {
delete kdTree_;
}
}
size_t RVOSimulator3D::getAgentNumAgentNeighbors(size_t agentNo) const
{
return agents_[agentNo]->agentNeighbors_.size();
}
size_t RVOSimulator3D::getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const
{
return agents_[agentNo]->agentNeighbors_[neighborNo].second->id_;
}
size_t RVOSimulator3D::getAgentNumORCAPlanes(size_t agentNo) const
{
return agents_[agentNo]->orcaPlanes_.size();
}
const Plane &RVOSimulator3D::getAgentORCAPlane(size_t agentNo, size_t planeNo) const
{
return agents_[agentNo]->orcaPlanes_[planeNo];
}
void RVOSimulator3D::removeAgent(size_t agentNo)
{
delete agents_[agentNo];
agents_[agentNo] = agents_.back();
agents_.pop_back();
}
size_t RVOSimulator3D::addAgent(const Vector3 &position)
{
if (defaultAgent_ == NULL) {
return RVO3D_ERROR;
}
Agent3D *agent = new Agent3D();
agent->position_ = position;
agent->maxNeighbors_ = defaultAgent_->maxNeighbors_;
agent->maxSpeed_ = defaultAgent_->maxSpeed_;
agent->neighborDist_ = defaultAgent_->neighborDist_;
agent->radius_ = defaultAgent_->radius_;
agent->timeHorizon_ = defaultAgent_->timeHorizon_;
agent->velocity_ = defaultAgent_->velocity_;
agent->id_ = agents_.size();
agents_.push_back(agent);
return agents_.size() - 1;
}
size_t RVOSimulator3D::addAgent(const Vector3 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
{
Agent3D *agent = new Agent3D();
agent->position_ = position;
agent->maxNeighbors_ = maxNeighbors;
agent->maxSpeed_ = maxSpeed;
agent->neighborDist_ = neighborDist;
agent->radius_ = radius;
agent->timeHorizon_ = timeHorizon;
agent->velocity_ = velocity;
agent->id_ = agents_.size();
agents_.push_back(agent);
return agents_.size() - 1;
}
void RVOSimulator3D::doStep()
{
kdTree_->buildAgentTree(agents_);
for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
agents_[i]->computeNeighbors(this);
agents_[i]->computeNewVelocity(this);
}
for (int i = 0; i < static_cast<int>(agents_.size()); ++i) {
agents_[i]->update(this);
}
globalTime_ += timeStep_;
}
size_t RVOSimulator3D::getAgentMaxNeighbors(size_t agentNo) const
{
return agents_[agentNo]->maxNeighbors_;
}
float RVOSimulator3D::getAgentMaxSpeed(size_t agentNo) const
{
return agents_[agentNo]->maxSpeed_;
}
float RVOSimulator3D::getAgentNeighborDist(size_t agentNo) const
{
return agents_[agentNo]->neighborDist_;
}
const Vector3 &RVOSimulator3D::getAgentPosition(size_t agentNo) const
{
return agents_[agentNo]->position_;
}
const Vector3 &RVOSimulator3D::getAgentPrefVelocity(size_t agentNo) const
{
return agents_[agentNo]->prefVelocity_;
}
float RVOSimulator3D::getAgentRadius(size_t agentNo) const
{
return agents_[agentNo]->radius_;
}
float RVOSimulator3D::getAgentTimeHorizon(size_t agentNo) const
{
return agents_[agentNo]->timeHorizon_;
}
const Vector3 &RVOSimulator3D::getAgentVelocity(size_t agentNo) const
{
return agents_[agentNo]->velocity_;
}
float RVOSimulator3D::getGlobalTime() const
{
return globalTime_;
}
size_t RVOSimulator3D::getNumAgents() const
{
return agents_.size();
}
float RVOSimulator3D::getTimeStep() const
{
return timeStep_;
}
void RVOSimulator3D::setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity)
{
if (defaultAgent_ == NULL) {
defaultAgent_ = new Agent3D();
}
defaultAgent_->maxNeighbors_ = maxNeighbors;
defaultAgent_->maxSpeed_ = maxSpeed;
defaultAgent_->neighborDist_ = neighborDist;
defaultAgent_->radius_ = radius;
defaultAgent_->timeHorizon_ = timeHorizon;
defaultAgent_->velocity_ = velocity;
}
void RVOSimulator3D::setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors)
{
agents_[agentNo]->maxNeighbors_ = maxNeighbors;
}
void RVOSimulator3D::setAgentMaxSpeed(size_t agentNo, float maxSpeed)
{
agents_[agentNo]->maxSpeed_ = maxSpeed;
}
void RVOSimulator3D::setAgentNeighborDist(size_t agentNo, float neighborDist)
{
agents_[agentNo]->neighborDist_ = neighborDist;
}
void RVOSimulator3D::setAgentPosition(size_t agentNo, const Vector3 &position)
{
agents_[agentNo]->position_ = position;
}
void RVOSimulator3D::setAgentPrefVelocity(size_t agentNo, const Vector3 &prefVelocity)
{
agents_[agentNo]->prefVelocity_ = prefVelocity;
}
void RVOSimulator3D::setAgentRadius(size_t agentNo, float radius)
{
agents_[agentNo]->radius_ = radius;
}
void RVOSimulator3D::setAgentTimeHorizon(size_t agentNo, float timeHorizon)
{
agents_[agentNo]->timeHorizon_ = timeHorizon;
}
void RVOSimulator3D::setAgentVelocity(size_t agentNo, const Vector3 &velocity)
{
agents_[agentNo]->velocity_ = velocity;
}
void RVOSimulator3D::setTimeStep(float timeStep)
{
timeStep_ = timeStep;
}
}

324
thirdparty/rvo2/rvo2_3d/RVOSimulator3d.h vendored
View File

@ -1,324 +0,0 @@
/*
* RVOSimulator.h
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* http://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <http://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file RVOSimulator.h
* \brief Contains the RVOSimulator class.
*/
#ifndef RVO3D_RVO_SIMULATOR_H_
#define RVO3D_RVO_SIMULATOR_H_
#include <cstddef>
#include <limits>
#include <vector>
#include "Vector3.h"
namespace RVO3D {
class Agent3D;
class KdTree3D;
/**
* \brief Error value.
*
* A value equal to the largest unsigned integer, which is returned in case of an error by functions in RVO3D::RVOSimulator.
*/
const size_t RVO3D_ERROR = std::numeric_limits<size_t>::max();
/**
* \brief Defines a plane.
*/
class Plane {
public:
/**
* \brief A point on the plane.
*/
Vector3 point;
/**
* \brief The normal to the plane.
*/
Vector3 normal;
};
/**
* \brief Defines the simulation.
*
* The main class of the library that contains all simulation functionality.
*/
class RVOSimulator3D {
public:
/**
* \brief Constructs a simulator instance.
*/
RVOSimulator3D();
/**
* \brief Constructs a simulator instance and sets the default properties for any new agent that is added.
* \param timeStep The time step of the simulation. Must be positive.
* \param neighborDist The default maximum distance (center point to center point) to other agents a new agent takes into account in the navigation. The larger this number, the longer he running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative.
* \param maxNeighbors The default maximum number of other agents a new agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
* \param timeHorizon The default minimum amount of time for which a new agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner an agent will respond to the presence of other agents, but the less freedom the agent has in choosing its velocities. Must be positive.
* \param radius The default radius of a new agent. Must be non-negative.
* \param maxSpeed The default maximum speed of a new agent. Must be non-negative.
* \param velocity The default initial three-dimensional linear velocity of a new agent (optional).
*/
RVOSimulator3D(float timeStep, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3());
/**
* \brief Destroys this simulator instance.
*/
~RVOSimulator3D();
/**
* \brief Adds a new agent with default properties to the simulation.
* \param position The three-dimensional starting position of this agent.
* \return The number of the agent, or RVO3D::RVO3D_ERROR when the agent defaults have not been set.
*/
size_t addAgent(const Vector3 &position);
/**
* \brief Adds a new agent to the simulation.
* \param position The three-dimensional starting position of this agent.
* \param neighborDist The maximum distance (center point to center point) to other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative.
* \param maxNeighbors The maximum number of other agents this agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
* \param timeHorizon The minimum amount of time for which this agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner this agent will respond to the presence of other agents, but the less freedom this agent has in choosing its velocities. Must be positive.
* \param radius The radius of this agent. Must be non-negative.
* \param maxSpeed The maximum speed of this agent. Must be non-negative.
* \param velocity The initial three-dimensional linear velocity of this agent (optional).
* \return The number of the agent.
*/
size_t addAgent(const Vector3 &position, float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3());
/**
* \brief Lets the simulator perform a simulation step and updates the three-dimensional position and three-dimensional velocity of each agent.
*/
void doStep();
/**
* \brief Returns the specified agent neighbor of the specified agent.
* \param agentNo The number of the agent whose agent neighbor is to be retrieved.
* \param neighborNo The number of the agent neighbor to be retrieved.
* \return The number of the neighboring agent.
*/
size_t getAgentAgentNeighbor(size_t agentNo, size_t neighborNo) const;
/**
* \brief Returns the maximum neighbor count of a specified agent.
* \param agentNo The number of the agent whose maximum neighbor count is to be retrieved.
* \return The present maximum neighbor count of the agent.
*/
size_t getAgentMaxNeighbors(size_t agentNo) const;
/**
* \brief Returns the maximum speed of a specified agent.
* \param agentNo The number of the agent whose maximum speed is to be retrieved.
* \return The present maximum speed of the agent.
*/
float getAgentMaxSpeed(size_t agentNo) const;
/**
* \brief Returns the maximum neighbor distance of a specified agent.
* \param agentNo The number of the agent whose maximum neighbor distance is to be retrieved.
* \return The present maximum neighbor distance of the agent.
*/
float getAgentNeighborDist(size_t agentNo) const;
/**
* \brief Returns the count of agent neighbors taken into account to compute the current velocity for the specified agent.
* \param agentNo The number of the agent whose count of agent neighbors is to be retrieved.
* \return The count of agent neighbors taken into account to compute the current velocity for the specified agent.
*/
size_t getAgentNumAgentNeighbors(size_t agentNo) const;
/**
* \brief Returns the count of ORCA constraints used to compute the current velocity for the specified agent.
* \param agentNo The number of the agent whose count of ORCA constraints is to be retrieved.
* \return The count of ORCA constraints used to compute the current velocity for the specified agent.
*/
size_t getAgentNumORCAPlanes(size_t agentNo) const;
/**
* \brief Returns the specified ORCA constraint of the specified agent.
* \param agentNo The number of the agent whose ORCA constraint is to be retrieved.
* \param planeNo The number of the ORCA constraint to be retrieved.
* \return A plane representing the specified ORCA constraint.
* \note The halfspace to which the normal of the plane points is the region of permissible velocities with respect to the specified ORCA constraint.
*/
const Plane &getAgentORCAPlane(size_t agentNo, size_t planeNo) const;
/**
* \brief Returns the three-dimensional position of a specified agent.
* \param agentNo The number of the agent whose three-dimensional position is to be retrieved.
* \return The present three-dimensional position of the (center of the) agent.
*/
const Vector3 &getAgentPosition(size_t agentNo) const;
/**
* \brief Returns the three-dimensional preferred velocity of a specified agent.
* \param agentNo The number of the agent whose three-dimensional preferred velocity is to be retrieved.
* \return The present three-dimensional preferred velocity of the agent.
*/
const Vector3 &getAgentPrefVelocity(size_t agentNo) const;
/**
* \brief Returns the radius of a specified agent.
* \param agentNo The number of the agent whose radius is to be retrieved.
* \return The present radius of the agent.
*/
float getAgentRadius(size_t agentNo) const;
/**
* \brief Returns the time horizon of a specified agent.
* \param agentNo The number of the agent whose time horizon is to be retrieved.
* \return The present time horizon of the agent.
*/
float getAgentTimeHorizon(size_t agentNo) const;
/**
* \brief Returns the three-dimensional linear velocity of a specified agent.
* \param agentNo The number of the agent whose three-dimensional linear velocity is to be retrieved.
* \return The present three-dimensional linear velocity of the agent.
*/
const Vector3 &getAgentVelocity(size_t agentNo) const;
/**
* \brief Returns the global time of the simulation.
* \return The present global time of the simulation (zero initially).
*/
float getGlobalTime() const;
/**
* \brief Returns the count of agents in the simulation.
* \return The count of agents in the simulation.
*/
size_t getNumAgents() const;
/**
* \brief Returns the time step of the simulation.
* \return The present time step of the simulation.
*/
float getTimeStep() const;
/**
* \brief Removes an agent from the simulation.
* \param agentNo The number of the agent that is to be removed.
* \note After the removal of the agent, the agent that previously had number getNumAgents() - 1 will now have number agentNo.
*/
void removeAgent(size_t agentNo);
/**
* \brief Sets the default properties for any new agent that is added.
* \param neighborDist The default maximum distance (center point to center point) to other agents a new agent takes into account in the navigation. The larger this number, the longer he running time of the simulation. If the number is too low, the simulation will not be safe. Must be non-negative.
* \param maxNeighbors The default maximum number of other agents a new agent takes into account in the navigation. The larger this number, the longer the running time of the simulation. If the number is too low, the simulation will not be safe.
* \param timeHorizon The default minimum amount of time for which a new agent's velocities that are computed by the simulation are safe with respect to other agents. The larger this number, the sooner an agent will respond to the presence of other agents, but the less freedom the agent has in choosing its velocities. Must be positive.
* \param radius The default radius of a new agent. Must be non-negative.
* \param maxSpeed The default maximum speed of a new agent. Must be non-negative.
* \param velocity The default initial three-dimensional linear velocity of a new agent (optional).
*/
void setAgentDefaults(float neighborDist, size_t maxNeighbors, float timeHorizon, float radius, float maxSpeed, const Vector3 &velocity = Vector3());
/**
* \brief Sets the maximum neighbor count of a specified agent.
* \param agentNo The number of the agent whose maximum neighbor count is to be modified.
* \param maxNeighbors The replacement maximum neighbor count.
*/
void setAgentMaxNeighbors(size_t agentNo, size_t maxNeighbors);
/**
* \brief Sets the maximum speed of a specified agent.
* \param agentNo The number of the agent whose maximum speed is to be modified.
* \param maxSpeed The replacement maximum speed. Must be non-negative.
*/
void setAgentMaxSpeed(size_t agentNo, float maxSpeed);
/**
* \brief Sets the maximum neighbor distance of a specified agent.
* \param agentNo The number of the agent whose maximum neighbor distance is to be modified.
* \param neighborDist The replacement maximum neighbor distance. Must be non-negative.
*/
void setAgentNeighborDist(size_t agentNo, float neighborDist);
/**
* \brief Sets the three-dimensional position of a specified agent.
* \param agentNo The number of the agent whose three-dimensional position is to be modified.
* \param position The replacement of the three-dimensional position.
*/
void setAgentPosition(size_t agentNo, const Vector3 &position);
/**
* \brief Sets the three-dimensional preferred velocity of a specified agent.
* \param agentNo The number of the agent whose three-dimensional preferred velocity is to be modified.
* \param prefVelocity The replacement of the three-dimensional preferred velocity.
*/
void setAgentPrefVelocity(size_t agentNo, const Vector3 &prefVelocity);
/**
* \brief Sets the radius of a specified agent.
* \param agentNo The number of the agent whose radius is to be modified.
* \param radius The replacement radius. Must be non-negative.
*/
void setAgentRadius(size_t agentNo, float radius);
/**
* \brief Sets the time horizon of a specified agent with respect to other agents.
* \param agentNo The number of the agent whose time horizon is to be modified.
* \param timeHorizon The replacement time horizon with respect to other agents. Must be positive.
*/
void setAgentTimeHorizon(size_t agentNo, float timeHorizon);
/**
* \brief Sets the three-dimensional linear velocity of a specified agent.
* \param agentNo The number of the agent whose three-dimensional linear velocity is to be modified.
* \param velocity The replacement three-dimensional linear velocity.
*/
void setAgentVelocity(size_t agentNo, const Vector3 &velocity);
/**
* \brief Sets the time step of the simulation.
* \param timeStep The time step of the simulation. Must be positive.
*/
void setTimeStep(float timeStep);
public:
Agent3D *defaultAgent_;
KdTree3D *kdTree_;
float globalTime_;
float timeStep_;
std::vector<Agent3D *> agents_;
friend class Agent3D;
friend class KdTree3D;
};
}
#endif

353
thirdparty/rvo2/rvo2_3d/Vector3.h vendored
View File

@ -1,353 +0,0 @@
/*
* Vector3.h
* RVO2-3D Library
*
* Copyright 2008 University of North Carolina at Chapel Hill
*
* Licensed under the Apache License, Version 2.0 (the "License");
* you may not use this file except in compliance with the License.
* You may obtain a copy of the License at
*
* https://www.apache.org/licenses/LICENSE-2.0
*
* Unless required by applicable law or agreed to in writing, software
* distributed under the License is distributed on an "AS IS" BASIS,
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
* See the License for the specific language governing permissions and
* limitations under the License.
*
* Please send all bug reports to <geom@cs.unc.edu>.
*
* The authors may be contacted via:
*
* Jur van den Berg, Stephen J. Guy, Jamie Snape, Ming C. Lin, Dinesh Manocha
* Dept. of Computer Science
* 201 S. Columbia St.
* Frederick P. Brooks, Jr. Computer Science Bldg.
* Chapel Hill, N.C. 27599-3175
* United States of America
*
* <https://gamma.cs.unc.edu/RVO2/>
*/
/**
* \file Vector3.h
* \brief Contains the Vector3 class.
*/
#ifndef RVO3D_VECTOR3_H_
#define RVO3D_VECTOR3_H_
#include <cmath>
#include <cstddef>
#include <ostream>
namespace RVO3D {
/**
* \brief Defines a three-dimensional vector.
*/
class Vector3 {
public:
/**
* \brief Constructs and initializes a three-dimensional vector instance to zero.
*/
inline Vector3()
{
val_[0] = 0.0f;
val_[1] = 0.0f;
val_[2] = 0.0f;
}
/**
* \brief Constructs and initializes a three-dimensional vector from the specified three-dimensional vector.
* \param vector The three-dimensional vector containing the xyz-coordinates.
*/
inline Vector3(const Vector3 &vector)
{
val_[0] = vector[0];
val_[1] = vector[1];
val_[2] = vector[2];
}
/**
* \brief Constructs and initializes a three-dimensional vector from the specified three-element array.
* \param val The three-element array containing the xyz-coordinates.
*/
inline explicit Vector3(const float val[3])
{
val_[0] = val[0];
val_[1] = val[1];
val_[2] = val[2];
}
/**
* \brief Constructs and initializes a three-dimensional vector from the specified xyz-coordinates.
* \param x The x-coordinate of the three-dimensional vector.
* \param y The y-coordinate of the three-dimensional vector.
* \param z The z-coordinate of the three-dimensional vector.
*/
inline Vector3(float x, float y, float z)
{
val_[0] = x;
val_[1] = y;
val_[2] = z;
}
/**
* \brief Returns the x-coordinate of this three-dimensional vector.
* \return The x-coordinate of the three-dimensional vector.
*/
inline float x() const { return val_[0]; }
/**
* \brief Returns the y-coordinate of this three-dimensional vector.
* \return The y-coordinate of the three-dimensional vector.
*/
inline float y() const { return val_[1]; }
/**
* \brief Returns the z-coordinate of this three-dimensional vector.
* \return The z-coordinate of the three-dimensional vector.
*/
inline float z() const { return val_[2]; }
/**
* \brief Returns the specified coordinate of this three-dimensional vector.
* \param i The coordinate that should be returned (0 <= i < 3).
* \return The specified coordinate of the three-dimensional vector.
*/
inline float operator[](size_t i) const { return val_[i]; }
/**
* \brief Returns a reference to the specified coordinate of this three-dimensional vector.
* \param i The coordinate to which a reference should be returned (0 <= i < 3).
* \return A reference to the specified coordinate of the three-dimensional vector.
*/
inline float &operator[](size_t i) { return val_[i]; }
/**
* \brief Computes the negation of this three-dimensional vector.
* \return The negation of this three-dimensional vector.
*/
inline Vector3 operator-() const
{
return Vector3(-val_[0], -val_[1], -val_[2]);
}
/**
* \brief Computes the dot product of this three-dimensional vector with the specified three-dimensional vector.
* \param vector The three-dimensional vector with which the dot product should be computed.
* \return The dot product of this three-dimensional vector with a specified three-dimensional vector.
*/
inline float operator*(const Vector3 &vector) const
{
return val_[0] * vector[0] + val_[1] * vector[1] + val_[2] * vector[2];
}
/**
* \brief Computes the scalar multiplication of this three-dimensional vector with the specified scalar value.
* \param scalar The scalar value with which the scalar multiplication should be computed.
* \return The scalar multiplication of this three-dimensional vector with a specified scalar value.
*/
inline Vector3 operator*(float scalar) const
{
return Vector3(val_[0] * scalar, val_[1] * scalar, val_[2] * scalar);
}
/**
* \brief Computes the scalar division of this three-dimensional vector with the specified scalar value.
* \param scalar The scalar value with which the scalar division should be computed.
* \return The scalar division of this three-dimensional vector with a specified scalar value.
*/
inline Vector3 operator/(float scalar) const
{
const float invScalar = 1.0f / scalar;
return Vector3(val_[0] * invScalar, val_[1] * invScalar, val_[2] * invScalar);
}
/**
* \brief Computes the vector sum of this three-dimensional vector with the specified three-dimensional vector.
* \param vector The three-dimensional vector with which the vector sum should be computed.
* \return The vector sum of this three-dimensional vector with a specified three-dimensional vector.
*/
inline Vector3 operator+(const Vector3 &vector) const
{
return Vector3(val_[0] + vector[0], val_[1] + vector[1], val_[2] + vector[2]);
}
/**
* \brief Computes the vector difference of this three-dimensional vector with the specified three-dimensional vector.
* \param vector The three-dimensional vector with which the vector difference should be computed.
* \return The vector difference of this three-dimensional vector with a specified three-dimensional vector.
*/
inline Vector3 operator-(const Vector3 &vector) const
{
return Vector3(val_[0] - vector[0], val_[1] - vector[1], val_[2] - vector[2]);
}
/**
* \brief Tests this three-dimensional vector for equality with the specified three-dimensional vector.
* \param vector The three-dimensional vector with which to test for equality.
* \return True if the three-dimensional vectors are equal.
*/
inline bool operator==(const Vector3 &vector) const
{
return val_[0] == vector[0] && val_[1] == vector[1] && val_[2] == vector[2];
}
/**
* \brief Tests this three-dimensional vector for inequality with the specified three-dimensional vector.
* \param vector The three-dimensional vector with which to test for inequality.
* \return True if the three-dimensional vectors are not equal.
*/
inline bool operator!=(const Vector3 &vector) const
{
return val_[0] != vector[0] || val_[1] != vector[1] || val_[2] != vector[2];
}
/**
* \brief Sets the value of this three-dimensional vector to the scalar multiplication of itself with the specified scalar value.
* \param scalar The scalar value with which the scalar multiplication should be computed.
* \return A reference to this three-dimensional vector.
*/
inline Vector3 &operator*=(float scalar)
{
val_[0] *= scalar;
val_[1] *= scalar;
val_[2] *= scalar;
return *this;
}
/**
* \brief Sets the value of this three-dimensional vector to the scalar division of itself with the specified scalar value.
* \param scalar The scalar value with which the scalar division should be computed.
* \return A reference to this three-dimensional vector.
*/
inline Vector3 &operator/=(float scalar)
{
const float invScalar = 1.0f / scalar;
val_[0] *= invScalar;
val_[1] *= invScalar;
val_[2] *= invScalar;
return *this;
}
/**
* \brief Sets the value of this three-dimensional vector to the vector
* sum of itself with the specified three-dimensional vector.
* \param vector The three-dimensional vector with which the vector sum should be computed.
* \return A reference to this three-dimensional vector.
*/
inline Vector3 &operator+=(const Vector3 &vector)
{
val_[0] += vector[0];
val_[1] += vector[1];
val_[2] += vector[2];
return *this;
}
/**
* \brief Sets the value of this three-dimensional vector to the vector difference of itself with the specified three-dimensional vector.
* \param vector The three-dimensional vector with which the vector difference should be computed.
* \return A reference to this three-dimensional vector.
*/
inline Vector3 &operator-=(const Vector3 &vector)
{
val_[0] -= vector[0];
val_[1] -= vector[1];
val_[2] -= vector[2];
return *this;
}
inline Vector3 &operator=(const Vector3 &vector)
{
val_[0] = vector[0];
val_[1] = vector[1];
val_[2] = vector[2];
return *this;
}
private:
float val_[3];
};
/**
* \relates Vector3
* \brief Computes the scalar multiplication of the specified three-dimensional vector with the specified scalar value.
* \param scalar The scalar value with which the scalar multiplication should be computed.
* \param vector The three-dimensional vector with which the scalar multiplication should be computed.
* \return The scalar multiplication of the three-dimensional vector with the scalar value.
*/
inline Vector3 operator*(float scalar, const Vector3 &vector)
{
return Vector3(scalar * vector[0], scalar * vector[1], scalar * vector[2]);
}
/**
* \relates Vector3
* \brief Computes the cross product of the specified three-dimensional vectors.
* \param vector1 The first vector with which the cross product should be computed.
* \param vector2 The second vector with which the cross product should be computed.
* \return The cross product of the two specified vectors.
*/
inline Vector3 cross(const Vector3 &vector1, const Vector3 &vector2)
{
return Vector3(vector1[1] * vector2[2] - vector1[2] * vector2[1], vector1[2] * vector2[0] - vector1[0] * vector2[2], vector1[0] * vector2[1] - vector1[1] * vector2[0]);
}
/**
* \relates Vector3
* \brief Inserts the specified three-dimensional vector into the specified output stream.
* \param os The output stream into which the three-dimensional vector should be inserted.
* \param vector The three-dimensional vector which to insert into the output stream.
* \return A reference to the output stream.
*/
inline std::ostream &operator<<(std::ostream &os, const Vector3 &vector)
{
os << "(" << vector[0] << "," << vector[1] << "," << vector[2] << ")";
return os;
}
/**
* \relates Vector3
* \brief Computes the length of a specified three-dimensional vector.
* \param vector The three-dimensional vector whose length is to be computed.
* \return The length of the three-dimensional vector.
*/
inline float abs(const Vector3 &vector)
{
return std::sqrt(vector * vector);
}
/**
* \relates Vector3
* \brief Computes the squared length of a specified three-dimensional vector.
* \param vector The three-dimensional vector whose squared length is to be computed.
* \return The squared length of the three-dimensional vector.
*/
inline float absSq(const Vector3 &vector)
{
return vector * vector;
}
/**
* \relates Vector3
* \brief Computes the normalization of the specified three-dimensional vector.
* \param vector The three-dimensional vector whose normalization is to be computed.
* \return The normalization of the three-dimensional vector.
*/
inline Vector3 normalize(const Vector3 &vector)
{
return vector / abs(vector);
}
}
#endif