imported Löve GLES branch (changeset 1ba9037e558b)

This commit is contained in:
Martin Felis
2013-12-05 18:05:13 +01:00
parent 41b2db04a7
commit 2644d1ee18
615 changed files with 150300 additions and 0 deletions
+58
View File
@@ -0,0 +1,58 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_DATA_H
#define LOVE_DATA_H
// LOVE
#include "config.h"
#include "Object.h"
namespace love
{
/**
* This class is a simple abstraction over all objects which contain data.
**/
class Data : public Object
{
public:
/**
* Destructor.
**/
virtual ~Data() {};
/**
* Gets a pointer to the data. This pointer will obviously not
* be valid if the Data object is destroyed.
**/
virtual void *getData() const = 0 ;
/**
* Gets the size of the Data in bytes.
**/
virtual int getSize() const = 0;
}; // Data
} // love
#endif // LOVE_DATA_H
+100
View File
@@ -0,0 +1,100 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_ENUM_MAP_H
#define LOVE_ENUM_MAP_H
#include "Exception.h"
namespace love
{
template<typename T, typename U, unsigned PEAK>
class EnumMap
{
public:
struct Entry
{
T t;
U u;
};
EnumMap(Entry *entries, unsigned size)
{
unsigned n = size/sizeof(Entry);
for (unsigned i = 0; i<n; ++i)
{
unsigned e_t = (unsigned)entries[i].t;
unsigned e_u = (unsigned)entries[i].u;
if (e_t < PEAK)
{
values_u[e_t].v = e_u;
values_u[e_t].set = true;
}
if (e_u < PEAK)
{
values_t[e_u].v = e_t;
values_t[e_u].set = true;
}
}
}
bool find(T t, U &u)
{
if ((unsigned)t < PEAK && values_u[(unsigned)t].set)
{
u = (U)values_u[(unsigned)t].v;
return true;
}
return false;
}
bool find(U u, T &t)
{
if ((unsigned)u < PEAK && values_t[(unsigned)u].set)
{
t = (T)values_t[(unsigned)u].v;
return true;
}
return false;
}
private:
struct Value
{
unsigned v;
bool set;
Value() : set(false) {}
};
Value values_t[PEAK];
Value values_u[PEAK];
}; // EnumMap
} // love
#endif // LOVE_ENUM_MAP_H
+63
View File
@@ -0,0 +1,63 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "Exception.h"
#include "common/config.h"
#include <iostream>
namespace love
{
Exception::Exception(const char *fmt, ...)
{
va_list args;
int size_buffer = 256, size_out;
char *buffer;
while (true)
{
buffer = new char[size_buffer];
memset(buffer, 0, size_buffer);
va_start(args, fmt);
size_out = vsnprintf(buffer, size_buffer, fmt, args);
va_end(args);
// see http://perfec.to/vsnprintf/pasprintf.c
// if size_out ...
// == -1 --> output was truncated
// == size_buffer --> output was truncated
// == size_buffer-1 --> ambiguous, /may/ have been truncated
// > size_buffer --> output was truncated, and size_out
// bytes would have been written
if (size_out == size_buffer || size_out == -1 || size_out == size_buffer-1)
size_buffer *= 2;
else if (size_out > size_buffer)
size_buffer = size_out + 2; // to avoid the ambiguous case
else
break;
delete[] buffer;
}
message = std::string(buffer);
delete[] buffer;
}
}
+67
View File
@@ -0,0 +1,67 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_EXCEPTION_H
#define LOVE_EXCEPTION_H
#include <exception>
#include <cstdarg> // vararg
#include <cstdio> // vsnprintf
#include <cstring> // strncpy
#include <string>
namespace love
{
/**
* A convenient vararg-enabled exception class.
**/
class Exception : public std::exception
{
public:
/**
* Creates a new Exception according to printf-rules.
*
* See: http://www.cplusplus.com/reference/clibrary/cstdio/printf/
*
* @param fmt The format string (see printf).
**/
Exception(const char *fmt, ...);
virtual ~Exception() throw() {}
/**
* Returns a string containing reason for the exception.
* @return A description of the exception.
**/
inline virtual const char *what() const throw()
{
return message.c_str();
}
private:
std::string message;
}; // Exception
} // love
#endif // LOVE_EXCEPTION_H
+212
View File
@@ -0,0 +1,212 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "Matrix.h"
// STD
#include <cstring> // memcpy
#include <cmath>
namespace love
{
// | e0 e4 e8 e12 |
// | e1 e5 e9 e13 |
// | e2 e6 e10 e14 |
// | e3 e7 e11 e15 |
Matrix::Matrix()
{
setIdentity();
}
Matrix::~Matrix()
{
}
// | e0 e4 e8 e12 |
// | e1 e5 e9 e13 |
// | e2 e6 e10 e14 |
// | e3 e7 e11 e15 |
// | e0 e4 e8 e12 |
// | e1 e5 e9 e13 |
// | e2 e6 e10 e14 |
// | e3 e7 e11 e15 |
Matrix Matrix::operator * (const Matrix &m) const
{
Matrix t;
t.e[0] = (e[0]*m.e[0]) + (e[4]*m.e[1]) + (e[8]*m.e[2]) + (e[12]*m.e[3]);
t.e[4] = (e[0]*m.e[4]) + (e[4]*m.e[5]) + (e[8]*m.e[6]) + (e[12]*m.e[7]);
t.e[8] = (e[0]*m.e[8]) + (e[4]*m.e[9]) + (e[8]*m.e[10]) + (e[12]*m.e[11]);
t.e[12] = (e[0]*m.e[12]) + (e[4]*m.e[13]) + (e[8]*m.e[14]) + (e[12]*m.e[15]);
t.e[1] = (e[1]*m.e[0]) + (e[5]*m.e[1]) + (e[9]*m.e[2]) + (e[13]*m.e[3]);
t.e[5] = (e[1]*m.e[4]) + (e[5]*m.e[5]) + (e[9]*m.e[6]) + (e[13]*m.e[7]);
t.e[9] = (e[1]*m.e[8]) + (e[5]*m.e[9]) + (e[9]*m.e[10]) + (e[13]*m.e[11]);
t.e[13] = (e[1]*m.e[12]) + (e[5]*m.e[13]) + (e[9]*m.e[14]) + (e[13]*m.e[15]);
t.e[2] = (e[2]*m.e[0]) + (e[6]*m.e[1]) + (e[10]*m.e[2]) + (e[14]*m.e[3]);
t.e[6] = (e[2]*m.e[4]) + (e[6]*m.e[5]) + (e[10]*m.e[6]) + (e[14]*m.e[7]);
t.e[10] = (e[2]*m.e[8]) + (e[6]*m.e[9]) + (e[10]*m.e[10]) + (e[14]*m.e[11]);
t.e[14] = (e[2]*m.e[12]) + (e[6]*m.e[13]) + (e[10]*m.e[14]) + (e[14]*m.e[15]);
t.e[3] = (e[3]*m.e[0]) + (e[7]*m.e[1]) + (e[11]*m.e[2]) + (e[15]*m.e[3]);
t.e[7] = (e[3]*m.e[4]) + (e[7]*m.e[5]) + (e[11]*m.e[6]) + (e[15]*m.e[7]);
t.e[11] = (e[3]*m.e[8]) + (e[7]*m.e[9]) + (e[11]*m.e[10]) + (e[15]*m.e[11]);
t.e[15] = (e[3]*m.e[12]) + (e[7]*m.e[13]) + (e[11]*m.e[14]) + (e[15]*m.e[15]);
return t;
}
void Matrix::operator *= (const Matrix &m)
{
Matrix t = (*this) * m;
memcpy((void *)this->e, (void *)t.e, sizeof(float)*16);
}
const float *Matrix::getElements() const
{
return e;
}
void Matrix::setIdentity()
{
memset(e, 0, sizeof(float)*16);
e[0] = e[5] = e[10] = e[15] = 1;
}
void Matrix::setTranslation(float x, float y)
{
setIdentity();
e[12] = x;
e[13] = y;
}
void Matrix::setRotation(float rad)
{
setIdentity();
float c = cosf(rad), s = sinf(rad);
e[0] = c;
e[4] = -s;
e[1] = s;
e[5] = c;
}
void Matrix::setScale(float sx, float sy)
{
setIdentity();
e[0] = sx;
e[5] = sy;
}
void Matrix::setShear(float kx, float ky)
{
setIdentity();
e[1] = ky;
e[4] = kx;
}
void Matrix::setTransformation(float x, float y, float angle, float sx, float sy, float ox, float oy, float kx, float ky)
{
memset(e, 0, sizeof(float)*16); // zero out matrix
float c = cosf(angle), s = sinf(angle);
// matrix multiplication carried out on paper:
// |1 x| |c -s | |sx | | 1 ky | |1 -ox|
// | 1 y| |s c | | sy | |kx 1 | | 1 -oy|
// | 1 | | 1 | | 1 | | 1 | | 1 |
// | 1| | 1| | 1| | 1| | 1 |
// move rotate scale skew origin
e[10] = e[15] = 1.0f;
e[0] = c * sx - ky * s * sy; // = a
e[1] = s * sx + ky * c * sy; // = b
e[4] = kx * c * sx - s * sy; // = c
e[5] = kx * s * sx + c * sy; // = d
e[12] = x - ox * e[0] - oy * e[4];
e[13] = y - ox * e[1] - oy * e[5];
}
void Matrix::translate(float x, float y)
{
Matrix t;
t.setTranslation(x, y);
this->operator *=(t);
}
void Matrix::rotate(float rad)
{
Matrix t;
t.setRotation(rad);
this->operator *=(t);
}
void Matrix::scale(float sx, float sy)
{
Matrix t;
t.setScale(sx, sy);
this->operator *=(t);
}
void Matrix::shear(float kx, float ky)
{
Matrix t;
t.setShear(kx,ky);
this->operator *=(t);
}
// | x |
// | y |
// | 0 |
// | 1 |
// | e0 e4 e8 e12 |
// | e1 e5 e9 e13 |
// | e2 e6 e10 e14 |
// | e3 e7 e11 e15 |
void Matrix::transform(Vertex *dst, const Vertex *src, int size) const
{
for (int i = 0; i<size; i++)
{
// Store in temp variables in case src = dst
float x = (e[0]*src[i].x) + (e[4]*src[i].y) + (0) + (e[12]);
float y = (e[1]*src[i].x) + (e[5]*src[i].y) + (0) + (e[13]);
dst[i].x = x;
dst[i].y = y;
}
}
Matrix Matrix::ortho(float left, float right, float bottom, float top)
{
Matrix m;
m.e[0] = 2.0f / (right - left);
m.e[5] = 2.0f / (top - bottom);
m.e[10] = -1.0;
m.e[12] = -(right + left) / (right - left);
m.e[13] = -(top + bottom) / (top - bottom);
return m;
}
} // love
+173
View File
@@ -0,0 +1,173 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_MATRIX_H
#define LOVE_MATRIX_H
// LOVE
#include "math.h"
namespace love
{
/**
* This class is the basis for all transformations in LOVE. Althought not
* really needed for 2D, it contains 4x4 elements to be compatible with
* OpenGL without conversions.
**/
class Matrix
{
public:
/**
* Creates a new identity matrix.
**/
Matrix();
/**
* Destructor.
**/
~Matrix();
/**
* Multiplies this Matrix with another Matrix, changing neither.
* @param m The Matrix to multiply with this Matrix.
* @return The combined matrix.
**/
Matrix operator * (const Matrix &m) const;
/**
* Multiplies a Matrix into this Matrix.
* @param m The Matrix to combine into this Matrix.
**/
void operator *= (const Matrix &m);
/**
* Gets a pointer to the 16 array elements.
* @return The array elements.
**/
const float *getElements() const;
/**
* Resets this Matrix to the identity matrix.
**/
void setIdentity();
/**
* Resets this Matrix to a translation.
* @param x Translation along x-axis.
* @param y Translation along y-axis.
**/
void setTranslation(float x, float y);
/**
* Resets this Matrix to a rotation.
* @param r The angle in radians.
**/
void setRotation(float r);
/**
* Resets this Matrix to a scale transformation.
* @param sx Scale factor along the x-axis.
* @param sy Scale factor along the y-axis.
**/
void setScale(float sx, float sy);
/**
* Resets this Matrix to a shear transformation.
* @param kx Shear along x-axis.
* @param ky Shear along y-axis.
**/
void setShear(float kx, float ky);
/**
* Creates a transformation with a certain position, orientation, scale
* and offset. Perfect for Drawables -- what a coincidence!
*
* @param x The translation along the x-axis.
* @param y The translation along the y-axis.
* @param angle The rotation (rad) around the center with offset (ox,oy).
* @param sx Scale along x-axis.
* @param sy Scale along y-axis.
* @param ox The offset for rotation along the x-axis.
* @param oy The offset for rotation along the y-axis.
* @param kx Shear along x-axis
* @param ky Shear along y-axis
**/
void setTransformation(float x, float y, float angle, float sx, float sy, float ox, float oy, float kx, float ky);
/**
* Multiplies this Matrix with a translation.
* @param x Translation along x-axis.
* @param y Translation along y-axis.
**/
void translate(float x, float y);
/**
* Multiplies this Matrix with a rotation.
* @param r Angle in radians.
**/
void rotate(float r);
/**
* Multiplies this Matrix with a scale transformation.
* @param sx Scale factor along the x-axis.
* @param sy Scale factor along the y-axis.
**/
void scale(float sx, float sy);
/**
* Multiplies this Matrix with a shear transformation.
* @param kx Shear along the x-axis.
* @param ky Shear along the y-axis.
**/
void shear(float kx, float ky);
/**
* Transforms an array of vertices by this Matrix. The sources and
* destination arrays may be the same.
*
* @param dst Storage for the transformed vertices.
* @param src The source vertices.
* @param size The number of vertices.
**/
void transform(Vertex *dst, const Vertex *src, int size) const;
/**
* Creates a new orthographic projection matrix with depth in the range of
* [-1, 1].
**/
static Matrix ortho(float left, float right, float bottom, float top);
private:
/**
* | e0 e4 e8 e12 |
* | e1 e5 e9 e13 |
* | e2 e6 e10 e14 |
* | e3 e7 e11 e15 |
**/
float e[16];
}; // Matrix
} //love
#endif// LOVE_MATRIX_H
+45
View File
@@ -0,0 +1,45 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "Memoizer.h"
#include <cstddef>
namespace love
{
std::map<void *, void *> Memoizer::objectMap;
void Memoizer::add(void *key, void *val)
{
objectMap[key] = val;
}
void Memoizer::remove(void *key)
{
objectMap.erase(key);
}
void *Memoizer::find(void *key)
{
if (objectMap.count(key)) return objectMap[key];
return NULL;
}
} // love
+46
View File
@@ -0,0 +1,46 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_MEMOIZER_H
#define LOVE_MEMOIZER_H
#include <map>
namespace love
{
class Memoizer
{
public:
static void add(void *key, void *val);
static void remove(void *key);
static void *find(void *key);
private:
static std::map<void *, void *> objectMap;
}; // Memoizer
} // love
#endif // LOVE_MEMOIZER_H
+124
View File
@@ -0,0 +1,124 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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.
**/
// LOVE
#include "Module.h"
#include "Exception.h"
// std
#include <map>
#include <utility>
#include <string>
namespace
{
typedef std::map<std::string, love::Module*> ModuleRegistry;
// The registry must be dynamically managed, because some modules
// (the math module) are static globals that are not guaranteed to
// be destroyed before other static globals.
ModuleRegistry *registry = nullptr;
ModuleRegistry &registryInstance()
{
if (!registry)
registry = new ModuleRegistry;
return *registry;
}
void freeEmptyRegistry()
{
if (registry && registry->empty())
{
delete registry;
registry = nullptr;
}
}
} // anonymous namespace
namespace love
{
Module::~Module()
{
ModuleRegistry &registry = registryInstance();
// We can't use the overridden Module::getName() in this destructor.
for (auto it = registry.begin(); it != registry.end(); ++it)
{
if (it->second == this)
{
registry.erase(it);
break;
}
}
freeEmptyRegistry();
}
void Module::registerInstance(Module *instance)
{
if (instance == nullptr)
throw Exception("Module instance is NULL");
std::string name(instance->getName());
ModuleRegistry &registry = registryInstance();
auto it = registry.find(name);
if (it != registry.end())
{
if (it->second == instance)
return;
throw Exception("Module %s already registered!", instance->getName());
}
registry.insert(make_pair(name, instance));
}
Module *Module::getInstance(const std::string &name)
{
ModuleRegistry &registry = registryInstance();
auto it = registry.find(name);
if (registry.end() == it)
return nullptr;
return it->second;
}
Module *Module::findInstance(const std::string &name)
{
ModuleRegistry &registry = registryInstance();
for (auto it = registry.begin(); it != registry.end(); ++it)
{
if (it->first.find(name) == 0)
return it->second;
}
return nullptr;
}
} // love
+79
View File
@@ -0,0 +1,79 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_MODULE_H
#define LOVE_MODULE_H
// LOVE
#include "runtime.h"
#include "Exception.h"
#include "Object.h"
namespace love
{
/**
* Abstract superclass for all modules.
**/
class Module : public Object
{
public:
/**
* Destructor.
**/
virtual ~Module();
/**
* Gets the name of the module. This is used in case of errors
* and other messages.
*
* @return The full name of the module, eg. love.graphics.opengl.
**/
virtual const char *getName() const = 0;
/**
* Add module to internal registry. To be used /only/ in
* runtime.cpp:luax_register_module()
* @param instance The module instance.
*/
static void registerInstance(Module *instance);
/**
* Retrieve module instance from internal registry. May return NULL
* if module not registered.
* @param name The full name of the module.
* @return Module instance or NULL if the module is not registered.
*/
static Module *getInstance(const std::string &name);
/**
* Find the first module instance from the internal registry whose name
* starts with the supplied name. May return NULL if module is not
* registered or the supplied name is not part of any module name.
* @param name The partial name of the module.
* @return Module instance or NULL if the module is not registered.
**/
static Module *findInstance(const std::string &name);
}; // Module
} // love
#endif // LOVE_MODULE_H
+52
View File
@@ -0,0 +1,52 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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.
**/
// LOVE
#include "Object.h"
namespace love
{
Object::Object()
: count(1)
{
}
Object::~Object()
{
}
int Object::getReferenceCount() const
{
return count;
}
void Object::retain()
{
++count;
}
void Object::release()
{
if (--count <= 0)
delete this;
}
} // love
+102
View File
@@ -0,0 +1,102 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_OBJECT_H
#define LOVE_OBJECT_H
namespace love
{
/**
* Superclass for all object that should be able to cross the Lua/C border
* (this pertains to most objects).
*
* This class is an alternative to using smart pointers; it contains retain/release
* methods, and will delete itself with the reference count hits zero. The wrapper
* code assumes that all userdata inherits from this class.
**/
class Object
{
public:
/**
* Constructor. Sets reference count to one.
**/
Object();
/**
* Destructor.
**/
virtual ~Object() = 0;
/**
* Gets the reference count of this Object.
* @returns The reference count.
**/
int getReferenceCount() const;
/**
* Retains the Object, i.e. increases the
* reference count by one.
**/
virtual void retain();
/**
* Releases one reference to the Object, i.e. decrements the
* reference count by one, and potentially deletes the Object
* if there are no more references.
**/
virtual void release();
/**
* Meant to be used as a temporary object. Facilitates safer and cleaner
* code to release objects by doing so when the AutoRelease object is
* destroyed (e.g. goes out of scope.)
**/
class AutoRelease
{
public:
AutoRelease(Object *obj)
: object(obj)
{
}
~AutoRelease()
{
if (object)
object->release();
}
private:
AutoRelease() {}
Object *object;
}; // AutoRelease
private:
// The reference count.
int count;
}; // Object
} // love
#endif // LOVE_OBJECT_H
+84
View File
@@ -0,0 +1,84 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "Reference.h"
namespace love
{
const char REFERENCE_TABLE_NAME[] = "love-references";
Reference::Reference()
: L(0)
, idx(LUA_REFNIL)
{
}
Reference::Reference(lua_State *L)
: L(L)
, idx(LUA_REFNIL)
{
ref(L);
}
Reference::~Reference()
{
unref();
}
void Reference::ref(lua_State *L)
{
unref(); // Just to be safe.
this->L = L;
luax_insist(L, LUA_REGISTRYINDEX, REFERENCE_TABLE_NAME);
lua_insert(L, -2); // Move reference table behind value.
idx = luaL_ref(L, -2);
lua_pop(L, 1);
}
void Reference::unref()
{
if (idx != LUA_REFNIL)
{
luax_insist(L, LUA_REGISTRYINDEX, REFERENCE_TABLE_NAME);
luaL_unref(L, -1, idx);
lua_pop(L, 1);
idx = LUA_REFNIL;
}
}
void Reference::push()
{
if (idx != LUA_REFNIL)
{
luax_insist(L, LUA_REGISTRYINDEX, REFERENCE_TABLE_NAME);
lua_rawgeti(L, -1, idx);
lua_remove(L, -2);
}
else
lua_pushnil(L);
}
lua_State *Reference::getL()
{
return L;
}
} // love
+88
View File
@@ -0,0 +1,88 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_REFERENCE_H
#define LOVE_REFERENCE_H
// LOVE
#include "runtime.h"
namespace love
{
/**
* This class wraps the reference functionality built into
* Lua, which allows C++ code to refer to Lua variables.
**/
class Reference
{
public:
/**
* Creates the reference object, but does not create
* the actual reference.
**/
Reference();
/**
* Creates the object and a reference to the value
* on the top of the stack.
**/
Reference(lua_State *L);
/**
* Deletes the reference, if any.
**/
virtual ~Reference();
/**
* Creates a reference to the value on the
* top of the stack.
**/
void ref(lua_State *L);
/**
* Unrefs the reference, if any.
**/
void unref();
/**
* Pushes the referred value onto the stack.
**/
void push();
/**
* Gets the Lua state associated with this
* reference.
**/
lua_State *getL();
private:
// The Lua state in which the reference resides.
lua_State *L;
// Index to the Lua reference.
int idx;
};
} // love
#endif // LOVE_REFERENCE_H
+168
View File
@@ -0,0 +1,168 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_STRING_MAP_H
#define LOVE_STRING_MAP_H
#include "Exception.h"
namespace love
{
template<typename T, unsigned SIZE>
class StringMap
{
public:
struct Entry
{
const char *key;
T value;
};
StringMap(Entry *entries, unsigned num)
{
for (unsigned i = 0; i < SIZE; ++i)
reverse[i] = 0;
unsigned n = num/sizeof(Entry);
for (unsigned i = 0; i < n; ++i)
{
add(entries[i].key, entries[i].value);
}
}
bool streq(const char *a, const char *b)
{
while (*a != 0 && *b != 0)
{
if (*a != *b)
return false;
++a;
++b;
}
return (*a == 0 && *b == 0);
}
bool find(const char *key, T &t)
{
unsigned str_hash = djb2(key);
for (unsigned i = 0; i < MAX; ++i)
{
unsigned str_i = (str_hash + i) % MAX;
if (!records[str_i].set)
return false;
if (streq(records[str_i].key, key))
{
t = records[str_i].value;
return true;
}
}
return false;
}
bool find(T key, const char *&str)
{
unsigned index = (unsigned)key;
if (index >= SIZE)
return false;
if (reverse[index] != 0)
{
str = reverse[index];
return true;
}
else
{
return false;
}
}
bool add(const char *key, T value)
{
unsigned str_hash = djb2(key);
bool inserted = false;
for (unsigned i = 0; i < MAX; ++i)
{
unsigned str_i = (str_hash + i) % MAX;
if (!records[str_i].set)
{
inserted = true;
records[str_i].set = true;
records[str_i].key = key;
records[str_i].value = value;
break;
}
}
unsigned index = (unsigned)value;
if (index >= SIZE)
{
printf("\nConstant %s out of bounds with %i!\n", key, index);
return false;
}
reverse[index] = key;
return inserted;
}
unsigned djb2(const char *key)
{
unsigned hash = 5381;
int c;
while ((c = *key++))
hash = ((hash << 5) + hash) + c;
return hash;
}
private:
struct Record
{
const char *key;
T value;
bool set;
Record() : set(false) {}
};
const static unsigned MAX = SIZE*2;
Record records[MAX];
const char *reverse[SIZE];
}; // StringMap
} // love
#endif // LOVE_STRING_MAP_H
+255
View File
@@ -0,0 +1,255 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "Variant.h"
#include "common/StringMap.h"
namespace love
{
extern StringMap<Type, TYPE_MAX_ENUM> types;
static love::Type extractudatatype(lua_State *L, int idx)
{
Type t = INVALID_ID;
if (!lua_isuserdata(L, idx))
return t;
if (luaL_getmetafield(L, idx, "__tostring") == 0)
return t;
lua_pushvalue(L, idx);
int result = lua_pcall(L, 1, 1, 0);
if (result == 0)
types.find(lua_tostring(L, -1), t);
if (result == 0 || result == LUA_ERRRUN)
lua_pop(L, 1);
return t;
}
static inline void delete_table(std::vector<std::pair<Variant*, Variant*> > *table)
{
while (!table->empty())
{
std::pair<Variant*, Variant*> &kv = table->back();
kv.first->release();
kv.second->release();
table->pop_back();
}
delete table;
}
Variant::Variant()
: type(NIL)
, data()
{
}
Variant::Variant(bool boolean)
: type(BOOLEAN)
{
data.boolean = boolean;
}
Variant::Variant(double number)
: type(NUMBER)
{
data.number = number;
}
Variant::Variant(const char *string, size_t len)
: type(STRING)
{
char *buf = new char[len+1];
memset(buf, 0, len+1);
memcpy(buf, string, len);
data.string.str = buf;
data.string.len = len;
}
Variant::Variant(char c)
: type(CHARACTER)
{
data.character = c;
}
Variant::Variant(void *userdata)
: type(LUSERDATA)
{
data.userdata = userdata;
}
Variant::Variant(love::Type udatatype, void *userdata)
: type(FUSERDATA)
, udatatype(udatatype)
{
if (udatatype != INVALID_ID)
{
Proxy *p = (Proxy *) userdata;
flags = p->flags;
data.userdata = p->data;
((love::Object *) data.userdata)->retain();
}
else
data.userdata = userdata;
}
// Variant gets ownership of the vector.
Variant::Variant(std::vector<std::pair<Variant*, Variant*> > *table)
: type(TABLE)
{
data.table = table;
}
Variant::~Variant()
{
switch (type)
{
case STRING:
delete[] data.string.str;
break;
case FUSERDATA:
((love::Object *) data.userdata)->release();
break;
case TABLE:
delete_table(data.table);
default:
break;
}
}
Variant *Variant::fromLua(lua_State *L, int n, bool allowTables)
{
Variant *v = NULL;
size_t len;
const char *str;
if (n < 0) // Fix the stack position, we might modify it later
n += lua_gettop(L) + 1;
switch (lua_type(L, n))
{
case LUA_TBOOLEAN:
v = new Variant(luax_toboolean(L, n));
break;
case LUA_TNUMBER:
v = new Variant(lua_tonumber(L, n));
break;
case LUA_TSTRING:
str = lua_tolstring(L, n, &len);
v = new Variant(str, len);
break;
case LUA_TLIGHTUSERDATA:
v = new Variant(lua_touserdata(L, n));
break;
case LUA_TUSERDATA:
v = new Variant(extractudatatype(L, n), lua_touserdata(L, n));
break;
case LUA_TNIL:
v = new Variant();
break;
case LUA_TTABLE:
if (allowTables)
{
bool success = true;
std::vector<std::pair<Variant*, Variant*> > *table = new std::vector<std::pair<Variant*, Variant*> >();
lua_pushnil(L);
while (lua_next(L, n))
{
Variant *key = fromLua(L, -2, false);
if (!key)
{
success = false;
lua_pop(L, 2);
break;
}
Variant *value = fromLua(L, -1, false);
if (!value)
{
delete key;
success = false;
lua_pop(L, 2);
break;
}
table->push_back(std::make_pair(key, value));
lua_pop(L, 1);
}
if (success)
v = new Variant(table);
else
delete_table(table);
}
break;
}
return v;
}
void Variant::toLua(lua_State *L)
{
switch (type)
{
case BOOLEAN:
lua_pushboolean(L, data.boolean);
break;
case CHARACTER:
lua_pushlstring(L, &data.character, 1);
break;
case NUMBER:
lua_pushnumber(L, data.number);
break;
case STRING:
lua_pushlstring(L, data.string.str, data.string.len);
break;
case LUSERDATA:
lua_pushlightuserdata(L, data.userdata);
break;
case FUSERDATA:
if (udatatype != INVALID_ID)
{
const char *name = NULL;
love::types.find(udatatype, name);
((love::Object *) data.userdata)->retain();
luax_pushtype(L, name, flags, (love::Object *) data.userdata);
}
else
lua_pushlightuserdata(L, data.userdata);
// I know this is not the same
// sadly, however, it's the most
// I can do (at the moment).
break;
case TABLE:
lua_newtable(L);
for (size_t i = 0; i < data.table->size(); ++i)
{
std::pair<Variant*, Variant*> &kv = data.table->at(i);
kv.first->toLua(L);
kv.second->toLua(L);
lua_settable(L, -3);
}
break;
case NIL:
default:
lua_pushnil(L);
break;
}
}
} // love
+83
View File
@@ -0,0 +1,83 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_VARIANT_H
#define LOVE_VARIANT_H
#include "common/runtime.h"
#include "common/Object.h"
#include <cstring>
#include <vector>
#include <utility>
namespace love
{
class Variant : public love::Object
{
public:
Variant();
Variant(bool boolean);
Variant(double number);
Variant(const char *string, size_t len);
Variant(char c);
Variant(void *userdata);
Variant(love::Type udatatype, void *userdata);
Variant(std::vector<std::pair<Variant*, Variant*> > *table);
virtual ~Variant();
static Variant *fromLua(lua_State *L, int n, bool allowTables = true);
void toLua(lua_State *L);
private:
enum Type
{
UNKNOWN = 0,
BOOLEAN,
NUMBER,
CHARACTER,
STRING,
LUSERDATA,
FUSERDATA,
NIL,
TABLE
} type;
union
{
bool boolean;
char character;
double number;
struct
{
const char *str;
size_t len;
} string;
void *userdata;
std::vector<std::pair<Variant*, Variant*> > *table;
} data;
love::Type udatatype;
bits flags;
}; // Variant
} // love
#endif // LOVE_VARIANT_H
+26
View File
@@ -0,0 +1,26 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "Vector.h"
namespace love
{
// Implementation in header.
}
+325
View File
@@ -0,0 +1,325 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_VECTOR_H
#define LOVE_VECTOR_H
// STD
#include <cmath>
// LOVE
#include "Matrix.h"
namespace love
{
/**
* 2D Vector class.
*
* @author Anders Ruud
* @date 2006-05-13
**/
class Vector
{
public:
// The components.
float x, y;
/**
* Creates a new (1,1) Vector.
**/
Vector();
/**
* Creates a new Vector.
* @param x The x position/dimension.
* @param y The y position/dimension.
**/
Vector(float x, float y);
/**
* Gets the length of the Vector.
* @return The length of the Vector.
*
* This method requires sqrtf() and should be used
* carefully.
**/
float getLength() const;
/**
* Normalizes the Vector.
* @param length Desired length of the vector.
* @return The old length of the Vector.
**/
float normalize(float length = 1.0);
/**
* Gets a vector perpendicular to the Vector.
* To get the true (normalized) normal, use v.getNormal(1.0f / v.getLength())
* @return A normal to the Vector.
**/
Vector getNormal() const;
/**
* Gets a vector perpendicular to the Vector.
* To get the true (normalized) normal, use v.getNormal(1.0f / v.getLength())
* @param scale factor to apply.
* @return A normal to the Vector.
**/
Vector getNormal(float scale) const;
/**
* Adds a Vector to this Vector.
* @param v The Vector we want to add to this Vector.
* @return The resulting Vector.
**/
Vector operator + (const Vector &v) const;
/**
* Substracts a Vector to this Vector.
* @param v The Vector we want to subtract to this Vector.
* @return The resulting Vector.
**/
Vector operator - (const Vector &v) const;
/**
* Resizes a Vector by a scalar.
* @param s The scalar with which to resize the Vector.
* @return The resulting Vector.
**/
Vector operator * (float s) const;
/**
* Resizes a Vector by a scalar.
* @param s The scalar with which to resize the Vector.
* @return The resulting Vector.
**/
Vector operator / (float s) const;
/**
* Reverses the Vector.
* @return The reversed Vector.
**/
Vector operator - () const;
/**
* Adds a Vector to this Vector, and also saves changes in the first Vector.
* @param v The Vector we want to add to this Vector.
**/
void operator += (const Vector &v);
/**
* Subtracts a Vector to this Vector, and also saves changes in the first Vector.
* @param v The Vector we want to subtract to this Vector.
**/
void operator -= (const Vector &v);
/**
* Resizes the Vector, and also saves changes in the first Vector.
* @param s The scalar by which we want to resize the Vector.
**/
void operator *= (float s);
/**
* Resizes the Vector, and also saves changes in the first Vector.
* @param s The scalar by which we want to resize the Vector.
**/
void operator /= (float s);
/**
* Calculates the dot product of two Vectors.
* @return The dot product of the two Vectors.
**/
float operator * (const Vector &v) const;
/**
* Calculates the cross product of two Vectors.
* @return The cross product of the two Vectors.
**/
float operator ^ (const Vector &v) const;
bool operator == (const Vector &v) const;
bool operator < (const Vector &v) const;
/**
* Gets the x value of the Vector.
* @return The x value of the Vector.
**/
float getX() const;
/**
* Gets the x value of the Vector.
* @return The x value of the Vector.
**/
float getY() const;
/**
* Sets the x value of the Vector.
* @param x The x value of the Vector.
**/
void setX(float x);
/**
* Sets the x value of the Vector.
* @param y The x value of the Vector.
**/
void setY(float y);
};
inline float Vector::getLength() const
{
return sqrtf(x*x + y*y);
}
inline Vector Vector::getNormal() const
{
return Vector(-y, x);
}
inline Vector Vector::getNormal(float scale) const
{
return Vector(-y * scale, x * scale);
}
inline float Vector::normalize(float length)
{
float length_current = getLength();
if (length_current > 0)
(*this) *= length / length_current;
return length_current;
}
/**
* Inline methods must have body in header.
**/
inline Vector::Vector()
{
x = 1;
y = 1;
}
inline Vector::Vector(float x, float y)
{
this->x = x;
this->y = y;
}
inline Vector Vector::operator + (const Vector &v) const
{
return Vector(x + v.x, y + v.y);
}
inline Vector Vector::operator - (const Vector &v) const
{
return Vector(x - v.getX(), y - v.getY());
}
inline Vector Vector::operator * (float s) const
{
return Vector(x*s, y*s);
}
inline Vector Vector::operator / (float s) const
{
return Vector(x/s, y/s);
}
inline Vector Vector::operator - () const
{
return Vector(-x, -y);
}
inline void Vector::operator += (const Vector &v)
{
x += v.getX();
y += v.getY();
}
inline void Vector::operator -= (const Vector &v)
{
x -= v.getX();
y -= v.getY();
}
inline void Vector::operator *= (float s)
{
x *= s;
y *= s;
}
inline void Vector::operator /= (float s)
{
x /= s;
y /= s;
}
inline float Vector::operator * (const Vector &v) const
{
return x * v.getX() + y * v.getY();
}
inline float Vector::operator ^ (const Vector &v) const
{
return x * v.getY() - y * v.getX();
}
inline bool Vector::operator == (const Vector &v) const
{
return getLength() == v.getLength();
}
inline bool Vector::operator < (const Vector &v) const
{
return getLength() < v.getLength();
}
/**
* Accessor methods
**/
inline float Vector::getX() const
{
return x;
}
inline float Vector::getY() const
{
return y;
}
inline void Vector::setX(float x)
{
this->x = x;
}
inline void Vector::setY(float y)
{
this->y = y;
}
} //love
#endif// LOVE_VECTOR_H
+80
View File
@@ -0,0 +1,80 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "b64.h"
namespace love
{
static const char cd64[]="|$$$}rstuvwxyz{$$$$$$$>?@ABCDEFGHIJKLMNOPQRSTUVW$$$$$$XYZ[\\]^_`abcdefghijklmnopq";
void b64_decode_block(char in[4], char out[3])
{
out[0] = (char)(in[0] << 2 | in[1] >> 4);
out[1] = (char)(in[1] << 4 | in[2] >> 2);
out[2] = (char)(((in[2] << 6) & 0xc0) | in[3]);
}
char *b64_decode(const char *src, int slen, int &size)
{
size = (slen / 4) * 3;
char *dst = new char[size];
char *d = dst;
char in[4] = {0}, out[3], v;
int i, len, pos = 0;
while (pos <= slen)
{
for (len = 0, i = 0; i < 4 && pos <= slen; i++)
{
v = 0;
while (pos <= slen && v == 0)
{
v = src[pos++];
v = (char)((v < 43 || v > 122) ? 0 : cd64[v - 43]);
if (v)
v = (char)((v == '$') ? 0 : v - 61);
}
if (pos <= slen)
{
len++;
if (v)
in[i] = (char)(v - 1);
}
else
in[i] = 0;
}
if (len)
{
b64_decode_block(in, out);
for (i = 0; i < len - 1; i++)
*(d++) = out[i];
}
}
return dst;
}
} // love
+41
View File
@@ -0,0 +1,41 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "config.h"
#ifndef LOVE_B64_H
#define LOVE_B64_H
namespace love
{
/**
* Decode base64 encoded data.
*
* @param src The string containing the base64 data.
* @param slen The length of the string.
* @param size The size of the binary data is stored here.
* @return A chunk of memory containing the binary data (allocated with new[]).
*/
char *b64_decode(const char *src, int slen, int &size);
} // love
#endif // LOVE_B64_H
+91
View File
@@ -0,0 +1,91 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_CONFIG_H
#define LOVE_CONFIG_H
// Platform stuff.
#if defined(WIN32) || defined(_WIN32)
# define LOVE_WINDOWS 1
#endif
#if defined(linux) || defined(__linux) || defined(__linux__)
# define LOVE_LINUX 1
#endif
#if defined(__APPLE__)
# define LOVE_MACOSX 1
# include <AvailabilityMacros.h>
#endif
// Endianness.
#if defined(__i386__) || defined(__i386)
# define LOVE_LITTLE_ENDIAN 1
#endif
#if defined(__ppc__) || defined(__ppc) || defined(__powerpc__) || defined(__powerpc)
# define LOVE_BIG_ENDIAN 1
#endif
// Warnings.
#ifndef _CRT_SECURE_NO_WARNINGS
# define _CRT_SECURE_NO_WARNINGS
#endif
// Preferably, and ironically, this macro should go unused.
#ifndef LOVE_UNUSED
# define LOVE_UNUSED(x) (void)sizeof(x)
#endif
#ifndef LOVE_BUILD
# define LOVE_BUILD
# define LOVE_BUILD_STANDALONE
# define LOVE_BUILD_EXE
//# define LOVE_BUILD_DLL
#endif
// DLL-stuff.
#ifdef LOVE_WINDOWS
# define LOVE_EXPORT __declspec(dllexport)
#else
# define LOVE_EXPORT
#endif
#if defined(LOVE_WINDOWS)
# define LOVE_LEGENDARY_UTF8_ARGV_HACK
# define LOVE_LEGENDARY_CONSOLE_IO_HACK
# define NOMINMAX
#endif
#if defined(LOVE_MACOSX)
# define LOVE_LEGENDARY_APP_ARGV_HACK
#endif
// Autotools config.h
#ifdef HAVE_CONFIG_H
# include <../config.h>
# undef VERSION
# ifdef WORDS_BIGENDIAN
# undef LOVE_LITTLE_ENDIAN
# define LOVE_BIG_ENDIAN 1
# else
# undef LOVE_BIG_ENDIAN
# define LOVE_LITTLE_ENDIAN 1
# endif
#endif
#endif // LOVE_CONFIG_H
+33
View File
@@ -0,0 +1,33 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "delay.h"
#include <SDL_timer.h>
namespace love
{
void delay(unsigned int ms)
{
SDL_Delay(ms);
}
} // love
+31
View File
@@ -0,0 +1,31 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 DELAY_H_
#define DELAY_H_
namespace love
{
void delay(unsigned int ms);
} // namespace love
#endif // DELAY_H_
+65
View File
@@ -0,0 +1,65 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_INT_H
#define LOVE_INT_H
#include "common/config.h"
#ifndef LOVE_WINDOWS
#include <stdint.h>
#endif
#define LOVE_INT8_MAX 0x7F
#define LOVE_UINT8_MAX 0xFF
#define LOVE_INT16_MAX 0x7FFF
#define LOVE_UINT16_MAX 0xFFFF
#define LOVE_INT32_MAX 0x7FFFFFFF
#define LOVE_UINT32_MAX 0xFFFFFFFF
#define LOVE_INT64_MAX 0x7FFFFFFFFFFFFFFF
#define LOVE_UINT64_MAX 0xFFFFFFFFFFFFFFFF
namespace love
{
// Blame Microsoft
#ifdef LOVE_WINDOWS
typedef __int8 int8;
typedef unsigned __int8 uint8;
typedef __int16 int16;
typedef unsigned __int16 uint16;
typedef __int32 int32;
typedef unsigned __int32 uint32;
typedef __int64 int64;
typedef unsigned __int64 uint64;
#else // LOVE_WINDOWS
typedef int8_t int8;
typedef uint8_t uint8;
typedef int16_t int16;
typedef uint16_t uint16;
typedef int32_t int32;
typedef uint32_t uint32;
typedef int64_t int64;
typedef uint64_t uint64;
#endif // LOVE_WINDOWS
} // love
#endif // LOVE_INT_H
+94
View File
@@ -0,0 +1,94 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_MATH_H
#define LOVE_MATH_H
#include <climits> // for CHAR_BIT
#include <cstdlib> // for rand() and RAND_MAX
/* Definitions of useful mathematical constants
* M_E - e
* M_LOG2E - log2(e)
* M_LOG10E - log10(e)
* M_LN2 - ln(2)
* M_LN10 - ln(10)
* M_PI - pi
* M_PI_2 - pi/2
* M_PI_4 - pi/4
* M_1_PI - 1/pi
* M_2_PI - 2/pi
* M_2_SQRTPI - 2/sqrt(pi)
* M_SQRT2 - sqrt(2)
* M_SQRT1_2 - 1/sqrt(2)
*/
#define LOVE_M_E 2.71828182845904523536
#define LOVE_M_LOG2E 1.44269504088896340736
#define LOVE_M_LOG10E 0.434294481903251827651
#define LOVE_M_LN2 0.693147180559945309417
#define LOVE_M_LN10 2.30258509299404568402
#define LOVE_M_PI 3.14159265358979323846
#define LOVE_M_PI_2 1.57079632679489661923
#define LOVE_M_PI_4 0.785398163397448309616
#define LOVE_M_1_PI 0.318309886183790671538
#define LOVE_M_2_PI 0.636619772367581343076
#define LOVE_M_2_SQRTPI 1.12837916709551257390
#define LOVE_M_SQRT2 1.41421356237309504880
#define LOVE_M_SQRT1_2 0.707106781186547524401
#define LOVE_M_TORAD (float)(LOVE_M_PI/180.0)
#define LOVE_M_TODEG (float)(180.0/LOVE_M_PI)
#define LOVE_TORAD(x) (float)(x*LOVE_M_TORAD)
#define LOVE_TODEG(x) (float)(x*LOVE_M_TODEG)
namespace love
{
struct Vertex
{
float x, y;
float s, t;
unsigned char r, g, b, a;
};
struct Triangle
{
Triangle(const Vertex &x, const Vertex &y, const Vertex &z)
: a(x), b(y), c(z)
{}
Vertex a, b, c;
};
inline int next_p2(int x)
{
x += (x == 0);
x--;
for (unsigned int i = 1; i < sizeof(int)*CHAR_BIT; i <<= 1) x |= x >> i;
return ++x;
}
inline float next_p2(float x)
{
return static_cast<float>(next_p2(static_cast<int>(x)));
}
} // love
#endif // LOVE_MATH_H
+757
View File
@@ -0,0 +1,757 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "config.h"
#include "runtime.h"
// LOVE
#include "Module.h"
#include "Object.h"
#include "Reference.h"
#include "StringMap.h"
#include <thread/threads.h>
// C++
#include <algorithm>
#include <iostream>
#include <cstdio>
namespace love
{
static thread::Mutex *gcmutex = 0;
void *_gcmutex = 0;
/**
* Called when an object is collected. The object is released
* once in this function, possibly deleting it.
**/
static int w__gc(lua_State *L)
{
if (!gcmutex)
{
gcmutex = thread::newMutex();
_gcmutex = (void *) gcmutex;
}
Proxy *p = (Proxy *)lua_touserdata(L, 1);
Object *t = (Object *)p->data;
thread::Lock lock(gcmutex);
int numretains = p->retains;
if (numretains >= 0)
numretains = std::min(numretains, t->getReferenceCount());
for (int i = numretains; i > 0; i--)
t->release();
// Signal that this Proxy is dead.
p->retains = -1;
return 0;
}
static int w__tostring(lua_State *L)
{
lua_pushvalue(L, lua_upvalueindex(1));
return 1;
}
static int w__typeOf(lua_State *L)
{
Proxy *p = (Proxy *)lua_touserdata(L, 1);
Type t = luax_type(L, 2);
luax_pushboolean(L, p->flags[t]);
return 1;
}
static int w__eq(lua_State *L)
{
Proxy *p1 = (Proxy *)lua_touserdata(L, 1);
Proxy *p2 = (Proxy *)lua_touserdata(L, 2);
luax_pushboolean(L, p1->data == p2->data);
return 1;
}
Reference *luax_refif(lua_State *L, int type)
{
Reference *r = 0;
// Create a reference only if the test succeeds.
if (lua_type(L, -1) == type)
r = new Reference(L);
else // Pop the value even if it fails (but also if it succeeds).
lua_pop(L, 1);
return r;
}
void luax_printstack(lua_State *L)
{
for (int i = 1; i<=lua_gettop(L); i++)
{
std::cout << i << " - " << luaL_typename(L, i) << std::endl;
}
}
bool luax_toboolean(lua_State *L, int idx)
{
return (lua_toboolean(L, idx) != 0);
}
void luax_pushboolean(lua_State *L, bool b)
{
lua_pushboolean(L, b ? 1 : 0);
}
bool luax_optboolean(lua_State *L, int idx, bool b)
{
if (lua_isboolean(L, idx) == 1)
return (lua_toboolean(L, idx) == 1 ? true : false);
return b;
}
std::string luax_tostring(lua_State *L, int idx)
{
size_t len;
const char *str = lua_tolstring(L, idx, &len);
return std::string(str, len);
}
std::string luax_checkstring(lua_State *L, int idx)
{
size_t len;
const char *str = luaL_checklstring(L, idx, &len);
return std::string(str, len);
}
void luax_pushstring(lua_State *L, const std::string &str)
{
lua_pushlstring(L, str.data(), str.size());
}
bool luax_boolflag(lua_State *L, int table_index, const char *key, bool defaultValue)
{
lua_getfield(L, table_index, key);
bool retval;
if (lua_isnoneornil(L, -1))
retval = defaultValue;
else
retval = lua_toboolean(L, -1);
lua_pop(L, 1);
return retval;
}
int luax_intflag(lua_State *L, int table_index, const char *key, int defaultValue)
{
lua_getfield(L, table_index, key);
int retval;
if (!lua_isnumber(L, -1))
retval = defaultValue;
else
retval = (int) lua_tointeger(L, -1);
lua_pop(L, 1);
return retval;
}
int luax_assert_argc(lua_State *L, int min)
{
int argc = lua_gettop(L);
if (argc < min)
return luaL_error(L, "Incorrect number of arguments. Got [%d], expected at least [%d]", argc, min);
return 0;
}
int luax_assert_argc(lua_State *L, int min, int max)
{
int argc = lua_gettop(L);
if (argc < min || argc > max)
return luaL_error(L, "Incorrect number of arguments. Got [%d], expected [%d-%d]", argc, min, max);
return 0;
}
int luax_assert_function(lua_State *L, int idx)
{
if (!lua_isfunction(L, idx))
return luaL_error(L, "Argument must be of type \"function\".");
return 0;
}
int luax_assert_nilerror(lua_State *L, int idx)
{
if (lua_isnoneornil(L, idx))
{
if (lua_isstring(L, idx + 1))
return luaL_error(L, lua_tostring(L, idx + 1));
else
return luaL_error(L, "assertion failed!");
}
return 0;
}
void luax_setfuncs(lua_State *L, const luaL_Reg *l)
{
if (l == 0)
return;
for (; l->name != 0; l++)
{
lua_pushcfunction(L, l->func);
lua_setfield(L, -2, l->name);
}
}
int luax_register_module(lua_State *L, const WrappedModule &m)
{
// Put a reference to the C++ module in Lua.
luax_insistregistry(L, REGISTRY_MODULES);
Proxy *p = (Proxy *)lua_newuserdata(L, sizeof(Proxy));
p->retains = 1;
p->data = m.module;
p->flags = m.flags;
luaL_newmetatable(L, m.module->getName());
lua_pushvalue(L, -1);
lua_setfield(L, -2, "__index");
lua_pushcfunction(L, w__gc);
lua_setfield(L, -2, "__gc");
lua_setmetatable(L, -2);
lua_setfield(L, -2, m.name); // _modules[name] = proxy
lua_pop(L, 1);
// Gets the love table.
luax_insistglobal(L, "love");
// Create new table for module.
lua_newtable(L);
// Register all the functions.
if (m.functions != 0)
luax_setfuncs(L, m.functions);
// Register types.
if (m.types != 0)
for (const lua_CFunction *t = m.types; *t != 0; t++)
(*t)(L);
lua_pushvalue(L, -1);
lua_setfield(L, -3, m.name); // love.graphics = table
lua_remove(L, -2); // love
// Register module instance
Module::registerInstance(m.module);
return 1;
}
int luax_preload(lua_State *L, lua_CFunction f, const char *name)
{
lua_getglobal(L, "package");
lua_getfield(L, -1, "preload");
lua_pushcfunction(L, f);
lua_setfield(L, -2, name);
lua_pop(L, 2);
return 0;
}
int luax_register_type(lua_State *L, const char *tname, const luaL_Reg *f)
{
// Verify that this type name has a matching Type ID and type name mapping.
love::Type ltype;
if (!love::getType(tname, ltype))
printf("Missing type entry for type name: %s\n", tname);
// Get the place for storing and re-using instantiated love types.
luax_getregistry(L, REGISTRY_TYPES);
// Create registry._lovetypes if it doesn't exist yet.
if (!lua_istable(L, -1))
{
lua_newtable(L);
lua_replace(L, -2);
// Create a metatable.
lua_newtable(L);
// metatable.__mode = "v". Weak userdata values.
lua_pushliteral(L, "v");
lua_setfield(L, -2, "__mode");
// setmetatable(newtable, metatable)
lua_setmetatable(L, -2);
// registry._lovetypes = newtable
lua_setfield(L, LUA_REGISTRYINDEX, "_lovetypes");
}
else
lua_pop(L, 1);
luaL_newmetatable(L, tname);
// m.__index = m
lua_pushvalue(L, -1);
lua_setfield(L, -2, "__index");
// setup gc
lua_pushcfunction(L, w__gc);
lua_setfield(L, -2, "__gc");
// Add equality
lua_pushcfunction(L, w__eq);
lua_setfield(L, -2, "__eq");
// Add tostring function.
lua_pushstring(L, tname);
lua_pushcclosure(L, w__tostring, 1);
lua_setfield(L, -2, "__tostring");
// Add tostring to as type() as well.
lua_pushstring(L, tname);
lua_pushcclosure(L, w__tostring, 1);
lua_setfield(L, -2, "type");
// Add typeOf
lua_pushcfunction(L, w__typeOf);
lua_setfield(L, -2, "typeOf");
if (f != 0)
luax_setfuncs(L, f);
lua_pop(L, 1); // Pops metatable.
return 0;
}
int luax_table_insert(lua_State *L, int tindex, int vindex, int pos)
{
if (tindex < 0)
tindex = lua_gettop(L)+1+tindex;
if (vindex < 0)
vindex = lua_gettop(L)+1+vindex;
if (pos == -1)
{
lua_pushvalue(L, vindex);
lua_rawseti(L, tindex, lua_objlen(L, tindex)+1);
return 0;
}
else if (pos < 0)
pos = lua_objlen(L, tindex)+1+pos;
for (int i = lua_objlen(L, tindex)+1; i > pos; i--)
{
lua_rawgeti(L, tindex, i-1);
lua_rawseti(L, tindex, i);
}
lua_pushvalue(L, vindex);
lua_rawseti(L, tindex, pos);
return 0;
}
int luax_register_searcher(lua_State *L, lua_CFunction f, int pos)
{
// Add the package loader to the package.loaders table.
lua_getglobal(L, "package");
if (lua_isnil(L, -1))
return luaL_error(L, "Can't register searcher: package table does not exist.");
lua_getfield(L, -1, "loaders");
// Lua 5.2 renamed package.loaders to package.searchers.
if (lua_isnil(L, -1))
{
lua_pop(L, 1);
lua_getfield(L, -1, "searchers");
}
if (lua_isnil(L, -1))
return luaL_error(L, "Can't register searcher: package.loaders table does not exist.");
lua_pushcfunction(L, f);
luax_table_insert(L, -2, -1, pos);
lua_pop(L, 3);
return 0;
}
void luax_rawnewtype(lua_State *L, const char *name, bits flags, love::Object *data, bool own)
{
Proxy *u = (Proxy *)lua_newuserdata(L, sizeof(Proxy));
u->data = (void *) data;
u->flags = flags;
u->retains = own ? 1 : 0;
luaL_newmetatable(L, name);
lua_setmetatable(L, -2);
}
void luax_pushtype(lua_State *L, const char *name, bits flags, love::Object *data, bool own)
{
// Fetch the registry table of instantiated types.
luax_getregistry(L, REGISTRY_TYPES);
// The table might not exist - it should be insisted in luax_register_type.
if (!lua_istable(L, -1))
{
lua_pop(L, 1);
return luax_rawnewtype(L, name, flags, data, own);
}
// Get the value of lovetypes[data] on the stack.
lua_pushlightuserdata(L, (void *) data);
lua_gettable(L, -2);
// If the Proxy userdata isn't in the instantiated types table yet, add it.
if (lua_type(L, -1) != LUA_TUSERDATA)
{
lua_pop(L, 1);
luax_rawnewtype(L, name, flags, data, own);
lua_pushlightuserdata(L, (void *) data);
lua_pushvalue(L, -2);
// lovetypes[data] = Proxy.
lua_settable(L, -4);
// Remove the lovetypes table from the stack.
lua_remove(L, -2);
// The Proxy userdata remains at the top of the stack.
return;
}
// Remove the lovetypes table from the stack.
lua_remove(L, -2);
// If the object should be released on GC and we already have a stored
// Proxy, we should tell the Proxy that the object was retained again.
if (own)
{
Proxy *p = (Proxy *) lua_touserdata(L, -1);
thread::EmptyLock lock;
if (gcmutex)
lock.setLock(gcmutex);
if (p->retains >= 0)
++(p->retains);
}
// Keep the Proxy userdata on the stack.
}
bool luax_istype(lua_State *L, int idx, love::bits type)
{
if (lua_isuserdata(L, idx) == 0)
return false;
return ((((Proxy *)lua_touserdata(L, idx))->flags & type) == type);
}
int luax_getfunction(lua_State *L, const char *mod, const char *fn)
{
lua_getglobal(L, "love");
if (lua_isnil(L, -1)) return luaL_error(L, "Could not find global love!");
lua_getfield(L, -1, mod);
if (lua_isnil(L, -1)) return luaL_error(L, "Could not find love.%s!", mod);
lua_getfield(L, -1, fn);
if (lua_isnil(L, -1)) return luaL_error(L, "Could not find love.%s.%s!", mod, fn);
lua_remove(L, -2); // remove mod
lua_remove(L, -2); // remove fn
return 0;
}
int luax_convobj(lua_State *L, int idx, const char *mod, const char *fn)
{
// Convert string to a file.
luax_getfunction(L, mod, fn);
lua_pushvalue(L, idx); // The initial argument.
lua_call(L, 1, 2); // Call the function, one arg, one return value (plus optional errstring.)
luax_assert_nilerror(L, -2); // Make sure the function returned something.
lua_pop(L, 1); // Pop the second return value now that we don't need it.
lua_replace(L, idx); // Replace the initial argument with the new object.
return 0;
}
int luax_convobj(lua_State *L, int idxs[], int n, const char *mod, const char *fn)
{
luax_getfunction(L, mod, fn);
for (int i = 0; i < n; i++)
{
lua_pushvalue(L, idxs[i]); // The arguments.
}
lua_call(L, n, 2); // Call the function, n args, one return value (plus optional errstring.)
luax_assert_nilerror(L, -2); // Make sure the function returned something.
lua_pop(L, 1); // Pop the second return value now that we don't need it.
lua_replace(L, idxs[0]); // Replace the initial argument with the new object.
return 0;
}
int luax_pconvobj(lua_State *L, int idx, const char *mod, const char *fn)
{
// Convert string to a file.
luax_getfunction(L, mod, fn);
lua_pushvalue(L, idx); // The initial argument.
int ret = lua_pcall(L, 1, 1, 0); // Call the function, one arg, one return value.
if (ret == 0)
lua_replace(L, idx); // Replace the initial argument with the new object.
return ret;
}
int luax_pconvobj(lua_State *L, int idxs[], int n, const char *mod, const char *fn)
{
luax_getfunction(L, mod, fn);
for (int i = 0; i < n; i++)
{
lua_pushvalue(L, idxs[i]); // The arguments.
}
int ret = lua_pcall(L, n, 1, 0); // Call the function, n args, one return value.
if (ret == 0)
lua_replace(L, idxs[0]); // Replace the initial argument with the new object.
return ret;
}
int luax_insist(lua_State *L, int idx, const char *k)
{
// Convert to absolute index if necessary.
if (idx < 0 && idx > LUA_REGISTRYINDEX)
idx = lua_gettop(L) + ++idx;
lua_getfield(L, idx, k);
// Create if necessary.
if (!lua_istable(L, -1))
{
lua_pop(L, 1); // Pop the non-table.
lua_newtable(L);
lua_pushvalue(L, -1); // Duplicate the table to leave on top.
lua_setfield(L, idx, k); // lua_stack[idx][k] = lua_stack[-1] (table)
}
return 1;
}
int luax_insistglobal(lua_State *L, const char *k)
{
lua_getglobal(L, k);
if (!lua_istable(L, -1))
{
lua_pop(L, 1); // Pop the non-table.
lua_newtable(L);
lua_pushvalue(L, -1);
lua_setglobal(L, k);
}
return 1;
}
int luax_insistlove(lua_State *L, const char *k)
{
luax_insistglobal(L, "love");
luax_insist(L, -1, k);
// The love table should be replaced with the top stack
// item. Only the reqested table should remain on the stack.
lua_replace(L, -2);
return 1;
}
int luax_getlove(lua_State *L, const char *k)
{
lua_getglobal(L, "love");
if (!lua_isnil(L, -1))
{
lua_getfield(L, -1, k);
lua_replace(L, -2);
}
return 1;
}
int luax_insistregistry(lua_State *L, Registry r)
{
switch (r)
{
case REGISTRY_GC:
return luax_insistlove(L, "_gc");
case REGISTRY_MODULES:
return luax_insistlove(L, "_modules");
case REGISTRY_TYPES:
return luax_insist(L, LUA_REGISTRYINDEX, "_lovetypes");
default:
return luaL_error(L, "Attempted to use invalid registry.");
}
}
int luax_getregistry(lua_State *L, Registry r)
{
switch (r)
{
case REGISTRY_GC:
return luax_getlove(L, "_gc");
case REGISTRY_MODULES:
return luax_getlove(L, "_modules");
case REGISTRY_TYPES:
lua_getfield(L, LUA_REGISTRYINDEX, "_lovetypes");
return 1;
default:
return luaL_error(L, "Attempted to use invalid registry.");
}
}
extern "C" int luax_typerror(lua_State *L, int narg, const char *tname)
{
int argtype = lua_type(L, narg);
const char *argtname = 0;
// We want to use the love type name for userdata, if possible.
if (argtype == LUA_TUSERDATA && luaL_getmetafield(L, narg, "__tostring") != 0)
{
lua_pushvalue(L, narg);
if (lua_pcall(L, 1, 1, 0) == 0 && lua_type(L, -1) == LUA_TSTRING)
{
argtname = lua_tostring(L, -1);
// Non-love userdata might have a tostring metamethod which doesn't
// describe its type, so we only use __tostring for love types.
love::Type t;
if (!love::getType(argtname, t))
argtname = 0;
}
}
if (argtname == 0)
argtname = lua_typename(L, argtype);
const char *msg = lua_pushfstring(L, "%s expected, got %s", tname, argtname);
return luaL_argerror(L, narg, msg);
}
StringMap<Type, TYPE_MAX_ENUM>::Entry typeEntries[] =
{
{"Invalid", INVALID_ID},
{"Object", OBJECT_ID},
{"Data", DATA_ID},
{"Module", MODULE_ID},
// Filesystem
{"File", FILESYSTEM_FILE_ID},
{"FileData", FILESYSTEM_FILE_DATA_ID},
// Font
{"GlyphData", FONT_GLYPH_DATA_ID},
{"Rasterizer", FONT_RASTERIZER_ID},
// Graphics
{"Drawable", GRAPHICS_DRAWABLE_ID},
{"Image", GRAPHICS_IMAGE_ID},
{"Quad", GRAPHICS_QUAD_ID},
{"Font", GRAPHICS_FONT_ID},
{"ParticleSystem", GRAPHICS_PARTICLE_SYSTEM_ID},
{"SpriteBatch", GRAPHICS_SPRITE_BATCH_ID},
{"Canvas", GRAPHICS_CANVAS_ID},
{"Shader", GRAPHICS_SHADER_ID},
{"Mesh", GRAPHICS_MESH_ID},
// Image
{"ImageData", IMAGE_IMAGE_DATA_ID},
{"CompressedData", IMAGE_COMPRESSED_DATA_ID},
// Joystick
{"Joystick", JOYSTICK_JOYSTICK_ID},
// Math
{"RandomGenerator", MATH_RANDOM_GENERATOR_ID},
{"BezierCurve", MATH_BEZIER_CURVE_ID},
// Audio
{"Source", AUDIO_SOURCE_ID},
// Sound
{"SoundData", SOUND_SOUND_DATA_ID},
{"Decoder", SOUND_DECODER_ID},
// Mouse
{"Cursor", MOUSE_CURSOR_ID},
// Physics
{"World", PHYSICS_WORLD_ID},
{"Contact", PHYSICS_CONTACT_ID},
{"Body", PHYSICS_BODY_ID},
{"Fixture", PHYSICS_FIXTURE_ID},
{"Shape", PHYSICS_SHAPE_ID},
{"CircleShape", PHYSICS_CIRCLE_SHAPE_ID},
{"PolygonShape", PHYSICS_POLYGON_SHAPE_ID},
{"EdgeShape", PHYSICS_EDGE_SHAPE_ID},
{"ChainShape", PHYSICS_CHAIN_SHAPE_ID},
{"Joint", PHYSICS_JOINT_ID},
{"MouseJoint", PHYSICS_MOUSE_JOINT_ID},
{"DistanceJoint", PHYSICS_DISTANCE_JOINT_ID},
{"PrismaticJoint", PHYSICS_PRISMATIC_JOINT_ID},
{"RevoluteJoint", PHYSICS_REVOLUTE_JOINT_ID},
{"PulleyJoint", PHYSICS_PULLEY_JOINT_ID},
{"GearJoint", PHYSICS_GEAR_JOINT_ID},
{"FrictionJoint", PHYSICS_FRICTION_JOINT_ID},
{"WeldJoint", PHYSICS_WELD_JOINT_ID},
{"RopeJoint", PHYSICS_ROPE_JOINT_ID},
{"WheelJoint", PHYSICS_WHEEL_JOINT_ID},
{"MotorJoint", PHYSICS_MOTOR_JOINT_ID},
// Thread
{"Thread", THREAD_THREAD_ID},
{"Channel", THREAD_CHANNEL_ID},
// The modules themselves. Only add abstracted modules here.
{"filesystem", MODULE_FILESYSTEM_ID},
{"graphics", MODULE_GRAPHICS_ID},
{"image", MODULE_IMAGE_ID},
{"sound", MODULE_SOUND_ID},
};
StringMap<Type, TYPE_MAX_ENUM> types(typeEntries, sizeof(typeEntries));
bool getType(const char *in, love::Type &out)
{
return types.find(in, out);
}
bool getType(love::Type in, const char *&out)
{
return types.find(in, out);
}
Type luax_type(lua_State *L, int idx)
{
Type t = INVALID_ID;
types.find(luaL_checkstring(L, idx), t);
return t;
}
} // love
+494
View File
@@ -0,0 +1,494 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_RUNTIME_H
#define LOVE_RUNTIME_H
// LOVE
#include "types.h"
#include "Object.h"
// Lua
extern "C" {
#define LUA_COMPAT_ALL
#include <lua.h>
#include <lualib.h>
#include <lauxlib.h>
}
namespace love
{
// Forward declarations.
class Module;
class Reference;
// Exposed mutex of the GC
extern void *_gcmutex;
/**
* Registries represent special tables which can be accessed with
* luax_insistregistry and luax_getregistry.
**/
enum Registry
{
REGISTRY_GC = 1,
REGISTRY_MODULES,
REGISTRY_TYPES
};
/**
* This structure wraps all Lua-exposed objects. It exists in the
* Lua state as a full userdata (so we can catch __gc "events"),
* though the Object it refers to is light userdata in the sense
* that it is not allocated by the Lua VM.
**/
struct Proxy
{
// Holds type information (see types.h).
bits flags;
// The light userdata (pointer to the love::Object).
void *data;
// The number of times release() should be called on GC.
int retains;
};
/**
* A Module with Lua wrapper functions and other data.
**/
struct WrappedModule
{
// The module containing the functions.
Module *module;
// The name for the table to put the functions in, without the 'love'-prefix.
const char *name;
// The type flags of this module.
love::bits flags;
// The functions of the module (last element {0,0}).
const luaL_Reg *functions;
// A list of functions which expose the types of the modules (last element 0).
const lua_CFunction *types;
};
/**
* Returns a reference to the top stack element (-1) if the value
* is of the specified type. If the value is incorrect, zero is returned.
*
* In any case, the top stack element is popped, regardless of its type.
**/
Reference *luax_refif(lua_State *L, int type);
/**
* Prints the current contents of the stack. Only useful for debugging.
* @param L The Lua state.
**/
void luax_printstack(lua_State *L);
/**
* Converts the value at idx to a bool. It follow the same rules
* as lua_toboolean, but returns a bool instead of an int.
* @param L The Lua state.
* @param idx The index on the Lua stack.
* @return True if the value evaluates to true, false otherwise.
**/
bool luax_toboolean(lua_State *L, int idx);
/**
* Pushes a bool onto the stack. It's the same as lua_pushboolean,
* but with bool instead of int.
* @param L The Lua state.
* @param b The bool to push.
**/
void luax_pushboolean(lua_State *L, bool b);
/**
* Converts the value at idx to a bool, or if not present, b is returned.
* @param L The Lua state.
* @param idx The index of the Lua stack.
* @param b The value to return if no value exist at the specified index.
* @return True if the value evaluates to true, false otherwise.
**/
bool luax_optboolean(lua_State *L, int idx, bool b);
/**
* Converts the value at idx to a std::string. It takes care of the string
* size and possible embedded nulls.
* @param L The Lua state.
* @param idx The index on the Lua stack.
* @return Copy of the string at the specified index.
**/
std::string luax_tostring(lua_State *L, int idx);
/**
* Converts the value at idx to a std::string. It takes care of the string
* size and possible embedded nulls.
* @param L The Lua state.
* @param idx The index on the Lua stack.
* @return Copy of the string at the specified index.
**/
std::string luax_checkstring(lua_State *L, int idx);
/**
* Pushes a std::string onto the stack. It uses the length of the string
* for lua_pushlstring's len argument.
* @param L The Lua state.
* @param str The string to push.
**/
void luax_pushstring(lua_State *L, const std::string &str);
bool luax_boolflag(lua_State *L, int table_index, const char *key, bool defaultValue);
int luax_intflag(lua_State *L, int table_index, const char *key, int defaultValue);
/**
* Convert the value at the specified index to an Lua number, and then
* convert to a float.
*
* @param L The Lua state.
* @param idx The index on the stack.
*/
inline float luax_tofloat(lua_State *L, int idx)
{
return static_cast<float>(lua_tonumber(L, idx));
}
/**
* Like luax_tofloat, but checks that the value is a number.
*
* @see luax_tofloat
*/
inline float luax_checkfloat(lua_State *L, int idx)
{
return static_cast<float>(luaL_checknumber(L, idx));
}
/**
* Require at least 'min' number of items on the stack.
* @param L The Lua state.
* @param min The minimum number of items on the stack.
* @return Zero if conditions are met, otherwise a Lua error (longjmp).
**/
int luax_assert_argc(lua_State *L, int min);
/**
* Require at least 'min', but more than 'max' items on the stack.
* @param L The Lua state.
* @param min The minimum number of items on the stack.
* @param max The maximum number of items on the stack.
* @return Zero if conditions are met, otherwise a Lua error (longjmp).
**/
int luax_assert_argc(lua_State *L, int min, int max);
/**
* Require that the value at idx is a function.
* @param L The Lua state.
*@param idx The index on the stack.
**/
int luax_assert_function(lua_State *L, int idx);
/**
* Require that the value at idx is not nil. If it is, the function throws an
* error using an optional error string at idx+1.
* @param L The Lua state.
* @param idx The index on the stack.
**/
int luax_assert_nilerror(lua_State *L, int idx);
/**
* Registers all functions in the array l (see luaL_Reg) into the table at the
* top of the stack.
* Similar to Lua 5.2's luaL_setfuncs without the upvalues, and to Lua 5.1's
* luaL_register without the library name.
**/
void luax_setfuncs(lua_State *L, const luaL_Reg *l);
/**
* Register a module in the love table. The love table will be created if it does not exist.
* @param L The Lua state.
**/
int luax_register_module(lua_State *L, const WrappedModule &m);
/**
* Inserts a module with 'name' into the package.preloaded table.
* @param f The function to be called when the module is opened.
* @param name The name of the module, with 'love'-prefix, for instance 'love.graphics'.
**/
int luax_preload(lua_State *L, lua_CFunction f, const char *name);
/**
* Register a new type.
* @param tname The name of the type. This must not conflict with other type names,
* even from other modules.
* @param f The list of member functions for the type.
**/
int luax_register_type(lua_State *L, const char *tname, const luaL_Reg *f = 0);
/**
* Do a table.insert from C
* @param L the state
* @param tindex the stack index of the table
* @param vindex the stack index of the value
* @param pos the position to insert it in
**/
int luax_table_insert(lua_State *L, int tindex, int vindex, int pos = -1);
/**
* Register a new searcher function for package.loaders. This can for instance enable
* loading of files through love.filesystem using standard require.
* @param L The Lua state.
* @param f The searcher function.
* @param pos The position to insert the loader in.
**/
int luax_register_searcher(lua_State *L, lua_CFunction f, int pos = -1);
/**
* Pushes a Lua representation of the given object onto the stack, creating and
* storing the Lua representation in a weak table if it doesn't exist yet.
* @param L The Lua state.
* @param name The name of the type. This must match the name used with luax_register_type.
* @param flags The type information of the object.
* @param data The pointer to the actual object.
* @param own Set this to true (default) if the object should be released upon garbage collection.
**/
void luax_pushtype(lua_State *L, const char *name, bits flags, love::Object *data, bool own = true);
/**
* Creates a new Lua representation of the given object *without* checking if it
* exists yet, and *without* storing it in a weak table.
* This should only be used when performance is an extreme concern and the
* object is not ever expected to be pushed to Lua again, as it prevents the
* Lua-side objects from working in all cases when used as keys in tables.
* @param L The Lua state.
* @param name The name of the type. This must match the name used with luax_register_type.
* @param flags The type information of the object.
* @param data The pointer to the actual object.
* @param own Set this to true (default) if the object should be released upon garbage collection.
**/
void luax_rawnewtype(lua_State *L, const char *name, bits flags, love::Object *data, bool own = true);
/**
* Checks whether the value at idx is a certain type.
* @param L The Lua state.
* @param idx The index on the stack.
* @param type The type to check for.
* @return True if the value is Proxy of the specified type, false otherwise.
**/
bool luax_istype(lua_State *L, int idx, love::bits type);
/**
* Gets the function love.module.function and puts it on top of the stack (alone). If the
* love table, the module, or the function does not exist, an error is returned.
* @return An error if nonexistent, or 1 if successful.
**/
int luax_getfunction(lua_State *L, const char *module, const char *function);
/**
* Converts an object into another object by the specified function love.module.function.
* The conversion function must accept a single object of the relevant type as a parameter,
* and returnone value. If the function does not exist (see luax_getfunction), an error is returned.
*
* Note that the initial object at idx is replaced by the new object.
*
* @param L The Lua state.
* @param idx The index on the stack.
* @param module The module in the love table.
* @param function The function in the module.
**/
int luax_convobj(lua_State *L, int idx, const char *module, const char *function);
/**
* Converts an object into another object by the specified function love.module.function.
* The conversion function must accept a single object of the relevant type as its first parameter,
* and return one value. If the function does not exist (see luax_getfunction), an error is returned.
*
* Note that the initial object at idx is replaced by the new object.
*
* @param L The Lua state.
* @param idxs An array of indices on the stack.
* @param n How many arguments are being passed.
* @param module The module in the love table.
* @param function The function in the module.
**/
int luax_convobj(lua_State *L, int idxs[], int n, const char *module, const char *function);
// pcall versions of the above
int luax_pconvobj(lua_State *L, int idx, const char *module, const char *function);
int luax_pconvobj(lua_State *L, int idxs[], int n, const char *module, const char *function);
/**
* 'Insist' that a table 'k' exists in the table at idx. Insistence involves that the
* table (k) is created if it does not exist in the table at idx. The table at idx must
* pre-exist, however. Also note that if the a non-table value exists at the specified
* location, it will be overwritten with a new table. The insisted table, and only the
* insisted table, will be placed on top of the stack.
*
* @param idx The index on the stack containing a table.
* @param k The name of the table we are insisting exist.
**/
int luax_insist(lua_State *L, int idx, const char *k);
/**
* Insist that a global table 'k' exists. See luax_insist.
* @param k The name of the table we are insisting exist.
**/
int luax_insistglobal(lua_State *L, const char *k);
/**
* Insists that a table 'k' exists inside the 'love' table. See luax_insist.
* @param k The name of the table we are insisting exist.
**/
int luax_insistlove(lua_State *L, const char *k);
/**
* Pushes the table 'k' in the love table onto the stack. Pushes nil if the
* table doesn't exist.
* @param k The name of the table we want to get.
**/
int luax_getlove(lua_State *L, const char *k);
/**
* Gets (creates if needed) the specified Registry, and pushes it into the
* stack.
* @param L The Lua state.
* @param r The Registry to get.
**/
int luax_insistregistry(lua_State *L, Registry r);
/**
* Gets the specified Registry, and pushes it onto the stack. Pushes nil if the
* registry hasn't been created (see luax_insistregistry.)
* @param L The Lua state.
* @param r The Registry to get.
**/
int luax_getregistry(lua_State *L, Registry r);
extern "C" { // Also called from luasocket
int luax_typerror(lua_State *L, int narg, const char *tname);
}
/**
* Like luax_totype, but causes an error if the value at idx is not Proxy,
* or is not the specified type.
* @param L The Lua state.
* @param idx The index on the stack.
* @param name The name of the type.
* @param type The type bit.
**/
template <typename T>
T *luax_checktype(lua_State *L, int idx, const char *name, love::bits type)
{
if (lua_isuserdata(L, idx) == 0)
luax_typerror(L, idx, name);
Proxy *u = (Proxy *)lua_touserdata(L, idx);
if ((u->flags & type) != type)
luax_typerror(L, idx, name);
return (T *)u->data;
}
template <typename T>
T *luax_getmodule(lua_State *L, const char *k, love::bits type)
{
luax_insistregistry(L, REGISTRY_MODULES);
lua_getfield(L, -1, k);
if (!lua_isuserdata(L, -1))
luaL_error(L, "Tried to get nonexistant module %s.", k);
Proxy *u = (Proxy *)lua_touserdata(L, -1);
if ((u->flags & type) != type)
luaL_error(L, "Incorrect module %s", k);
lua_pop(L, 2);
return (T *)u->data;
}
template <typename T>
T *luax_optmodule(lua_State *L, const char *k, love::bits type)
{
luax_insistregistry(L, REGISTRY_MODULES);
lua_getfield(L, -1, k);
if (!lua_isuserdata(L, -1))
{
lua_pop(L, 2);
return 0;
}
Proxy *u = (Proxy *)lua_touserdata(L, -1);
if ((u->flags & type) != type)
luaL_error(L, "Incorrect module %s", k);
lua_pop(L, 2);
return (T *) u->data;
}
/**
* Converts the value at idx to the specified type without checking that
* this conversion is valid. If the type has been previously verified with
* luax_istype, then this can be safely used. Otherwise, use luax_checktype.
* @param L The Lua state.
* @param idx The index on the stack.
* @param name The name of the type.
* @param type The type bit.
**/
template <typename T>
T *luax_totype(lua_State *L, int idx, const char * /* name */, love::bits /* type */)
{
return (T *)(((Proxy *)lua_touserdata(L, idx))->data);
}
Type luax_type(lua_State *L, int idx);
/**
* Macro for converting a LOVE exception into a Lua error.
* lua_error (and luaL_error) cannot be called from inside the exception handler
* because they use longjmp, which causes undefined behaviour when the
* destructor of the exception would have been called.
**/
#define EXCEPT_GUARD(A) \
{ \
bool should_error = false; \
try { A } \
catch (love::Exception &e) \
{ \
should_error = true; \
lua_pushstring(L, e.what()); \
} \
if (should_error) \
return luaL_error(L, "%s", lua_tostring(L, -1)); \
}
} // love
#endif // LOVE_RUNTIME_H
+202
View File
@@ -0,0 +1,202 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_TYPES_H
#define LOVE_TYPES_H
// STD
#include <bitset>
namespace love
{
enum Type
{
INVALID_ID = 0,
// Cross-module types.
OBJECT_ID,
DATA_ID,
MODULE_ID,
// Filesystem.
FILESYSTEM_FILE_ID,
FILESYSTEM_FILE_DATA_ID,
// Font
FONT_GLYPH_DATA_ID,
FONT_RASTERIZER_ID,
// Graphics
GRAPHICS_DRAWABLE_ID,
GRAPHICS_DRAWQABLE_ID,
GRAPHICS_IMAGE_ID,
GRAPHICS_QUAD_ID,
GRAPHICS_FONT_ID,
GRAPHICS_PARTICLE_SYSTEM_ID,
GRAPHICS_SPRITE_BATCH_ID,
GRAPHICS_CANVAS_ID,
GRAPHICS_SHADER_ID,
GRAPHICS_MESH_ID,
// Image
IMAGE_IMAGE_DATA_ID,
IMAGE_COMPRESSED_DATA_ID,
// Joystick
JOYSTICK_JOYSTICK_ID,
// Math
MATH_RANDOM_GENERATOR_ID,
MATH_BEZIER_CURVE_ID,
// Audio
AUDIO_SOURCE_ID,
// Sound
SOUND_SOUND_DATA_ID,
SOUND_DECODER_ID,
// Mouse
MOUSE_CURSOR_ID,
// Physics
PHYSICS_WORLD_ID,
PHYSICS_CONTACT_ID,
PHYSICS_BODY_ID,
PHYSICS_FIXTURE_ID,
PHYSICS_SHAPE_ID,
PHYSICS_CIRCLE_SHAPE_ID,
PHYSICS_POLYGON_SHAPE_ID,
PHYSICS_EDGE_SHAPE_ID,
PHYSICS_CHAIN_SHAPE_ID,
PHYSICS_JOINT_ID,
PHYSICS_MOUSE_JOINT_ID,
PHYSICS_DISTANCE_JOINT_ID,
PHYSICS_PRISMATIC_JOINT_ID,
PHYSICS_REVOLUTE_JOINT_ID,
PHYSICS_PULLEY_JOINT_ID,
PHYSICS_GEAR_JOINT_ID,
PHYSICS_FRICTION_JOINT_ID,
PHYSICS_WELD_JOINT_ID,
PHYSICS_ROPE_JOINT_ID,
PHYSICS_WHEEL_JOINT_ID,
PHYSICS_MOTOR_JOINT_ID,
// Thread
THREAD_THREAD_ID,
THREAD_CHANNEL_ID,
// The modules themselves. Only add abstracted modules here.
MODULE_FILESYSTEM_ID,
MODULE_GRAPHICS_ID,
MODULE_IMAGE_ID,
MODULE_SOUND_ID,
// Count the number of bits needed.
TYPE_MAX_ENUM
};
typedef std::bitset<TYPE_MAX_ENUM> bits;
const bits INVALID_T = bits(1) << INVALID_ID;
const bits OBJECT_T = bits(1) << OBJECT_ID;
const bits DATA_T = (bits(1) << DATA_ID) | OBJECT_T;
const bits MODULE_T = (bits(1) << MODULE_ID) | OBJECT_T;
// Filesystem.
const bits FILESYSTEM_FILE_T = (bits(1) << FILESYSTEM_FILE_ID) | OBJECT_T;
const bits FILESYSTEM_FILE_DATA_T = (bits(1) << FILESYSTEM_FILE_DATA_ID) | DATA_T;
const bits FONT_GLYPH_DATA_T = (bits(1) << FONT_GLYPH_DATA_ID) | DATA_T;
const bits FONT_RASTERIZER_T = (bits(1) << FONT_RASTERIZER_ID) | OBJECT_T;
// Graphics.
const bits GRAPHICS_DRAWABLE_T = (bits(1) << GRAPHICS_DRAWABLE_ID) | OBJECT_T;
const bits GRAPHICS_DRAWQABLE_T = (bits(1) << GRAPHICS_DRAWQABLE_ID) | GRAPHICS_DRAWABLE_T;
const bits GRAPHICS_IMAGE_T = (bits(1) << GRAPHICS_IMAGE_ID) | GRAPHICS_DRAWQABLE_T;
const bits GRAPHICS_QUAD_T = (bits(1) << GRAPHICS_QUAD_ID) | OBJECT_T;
const bits GRAPHICS_FONT_T = (bits(1) << GRAPHICS_FONT_ID) | OBJECT_T;
const bits GRAPHICS_PARTICLE_SYSTEM_T = (bits(1) << GRAPHICS_PARTICLE_SYSTEM_ID) | GRAPHICS_DRAWABLE_T;
const bits GRAPHICS_SPRITE_BATCH_T = (bits(1) << GRAPHICS_SPRITE_BATCH_ID) | GRAPHICS_DRAWABLE_T;
const bits GRAPHICS_CANVAS_T = (bits(1) << GRAPHICS_CANVAS_ID) | GRAPHICS_DRAWQABLE_T;
const bits GRAPHICS_SHADER_T = (bits(1) << GRAPHICS_SHADER_ID) | OBJECT_T;
const bits GRAPHICS_MESH_T = (bits(1) << GRAPHICS_MESH_ID) | GRAPHICS_DRAWABLE_T;
// Image.
const bits IMAGE_IMAGE_DATA_T = (bits(1) << IMAGE_IMAGE_DATA_ID) | DATA_T;
const bits IMAGE_COMPRESSED_DATA_T = (bits(1) << IMAGE_COMPRESSED_DATA_ID) | DATA_T;
// Joystick.
const bits JOYSTICK_JOYSTICK_T = (bits(1) << JOYSTICK_JOYSTICK_ID) | OBJECT_T;
// Math.
const bits MATH_RANDOM_GENERATOR_T = (bits(1) << MATH_RANDOM_GENERATOR_ID) | OBJECT_T;
const bits MATH_BEZIER_CURVE_T = (bits(1) << MATH_BEZIER_CURVE_ID) | OBJECT_T;
// Audio.
const bits AUDIO_SOURCE_T = (bits(1) << AUDIO_SOURCE_ID) | OBJECT_T;
// Sound.
const bits SOUND_SOUND_DATA_T = (bits(1) << SOUND_SOUND_DATA_ID) | DATA_T;
const bits SOUND_DECODER_T = bits(1) << SOUND_DECODER_ID;
// Mouse.
const bits MOUSE_CURSOR_T = (bits(1) << MOUSE_CURSOR_ID) | OBJECT_T;
// Physics.
const bits PHYSICS_WORLD_T = (bits(1) << PHYSICS_WORLD_ID) | OBJECT_T;
const bits PHYSICS_CONTACT_T = (bits(1) << PHYSICS_CONTACT_ID) | OBJECT_T;
const bits PHYSICS_BODY_T = (bits(1) << PHYSICS_BODY_ID) | OBJECT_T;
const bits PHYSICS_FIXTURE_T = (bits(1) << PHYSICS_FIXTURE_ID) | OBJECT_T;
const bits PHYSICS_SHAPE_T = (bits(1) << PHYSICS_SHAPE_ID) | OBJECT_T;
const bits PHYSICS_CIRCLE_SHAPE_T = (bits(1) << PHYSICS_CIRCLE_SHAPE_ID) | PHYSICS_SHAPE_T;
const bits PHYSICS_POLYGON_SHAPE_T = (bits(1) << PHYSICS_POLYGON_SHAPE_ID) | PHYSICS_SHAPE_T;
const bits PHYSICS_EDGE_SHAPE_T = (bits(1) << PHYSICS_EDGE_SHAPE_ID) | PHYSICS_SHAPE_T;
const bits PHYSICS_CHAIN_SHAPE_T = (bits(1) << PHYSICS_CHAIN_SHAPE_ID) | PHYSICS_SHAPE_T;
const bits PHYSICS_JOINT_T = (bits(1) << PHYSICS_JOINT_ID) | OBJECT_T;
const bits PHYSICS_MOUSE_JOINT_T = (bits(1) << PHYSICS_MOUSE_JOINT_ID) | PHYSICS_JOINT_T;
const bits PHYSICS_DISTANCE_JOINT_T = (bits(1) << PHYSICS_DISTANCE_JOINT_ID) | PHYSICS_JOINT_T;
const bits PHYSICS_PRISMATIC_JOINT_T = (bits(1) << PHYSICS_PRISMATIC_JOINT_ID) | PHYSICS_JOINT_T;
const bits PHYSICS_REVOLUTE_JOINT_T = (bits(1) << PHYSICS_REVOLUTE_JOINT_ID) | PHYSICS_JOINT_T;
const bits PHYSICS_PULLEY_JOINT_T = (bits(1) << PHYSICS_PULLEY_JOINT_ID) | PHYSICS_JOINT_T;
const bits PHYSICS_GEAR_JOINT_T = (bits(1) << PHYSICS_GEAR_JOINT_ID) | PHYSICS_JOINT_T;
const bits PHYSICS_FRICTION_JOINT_T = (bits(1) << PHYSICS_FRICTION_JOINT_ID) | PHYSICS_JOINT_T;
const bits PHYSICS_WELD_JOINT_T = (bits(1) << PHYSICS_WELD_JOINT_ID) | PHYSICS_JOINT_T;
const bits PHYSICS_ROPE_JOINT_T = (bits(1) << PHYSICS_ROPE_JOINT_ID) | PHYSICS_JOINT_T;
const bits PHYSICS_WHEEL_JOINT_T = (bits(1) << PHYSICS_WHEEL_JOINT_ID) | PHYSICS_JOINT_T;
const bits PHYSICS_MOTOR_JOINT_T = (bits(1) << PHYSICS_MOTOR_JOINT_ID) | PHYSICS_JOINT_T;
// Thread.
const bits THREAD_THREAD_T = (bits(1) << THREAD_THREAD_ID) | OBJECT_T;
const bits THREAD_CHANNEL_T = (bits(1) << THREAD_CHANNEL_ID) | OBJECT_T;
// Modules.
const bits MODULE_FILESYSTEM_T = (bits(1) << MODULE_FILESYSTEM_ID) | MODULE_T;
const bits MODULE_GRAPHICS_T = (bits(1) << MODULE_GRAPHICS_ID) | MODULE_T;
const bits MODULE_IMAGE_T = (bits(1) << MODULE_IMAGE_ID) | MODULE_T;
const bits MODULE_SOUND_T = (bits(1) << MODULE_SOUND_ID) | MODULE_T;
bool getType(const char *in, Type &out);
bool getType(Type in, const char *&out);
} // love
#endif // LOVE_TYPES_H
+61
View File
@@ -0,0 +1,61 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "utf8.h"
#ifdef LOVE_WINDOWS
namespace love
{
std::string to_utf8(LPCWSTR wstr)
{
size_t wide_len = wcslen(wstr)+1;
// Get size in UTF-8.
int utf8_size = WideCharToMultiByte(CP_UTF8, 0, wstr, wide_len, 0, 0, 0, 0);
char *utf8_str = new char[utf8_size];
// Convert to UTF-8.
int ok = WideCharToMultiByte(CP_UTF8, 0, wstr, wide_len, utf8_str, utf8_size, 0, 0);
std::string ret;
if (ok)
ret = utf8_str;
delete[] utf8_str;
return ret;
}
void replace_char(std::string &str, char find, char replace)
{
int length = str.length();
for (int i = 0; i<length; i++)
{
if (str[i] == find)
str[i] = replace;
}
}
} // love
#endif // LOVE_WINDOWS
+48
View File
@@ -0,0 +1,48 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "config.h"
#ifdef LOVE_WINDOWS
#include <string>
#include <windows.h>
namespace love
{
/**
* Convert the wide string to a UTF-8 encoded string.
* @param wstr The wide-char string.
* @return A UTF-8 string.
**/
std::string to_utf8(LPCWSTR wstr);
/**
* Replace all occurences of 'find' with 'replace' in a string.
* @param str The string to modify.
* @param find The character to match.
* @param replace The character to replace matches.
**/
void replace_char(std::string &str, char find, char replace);
} // love
#endif // LOVE_WINDOWS
+37
View File
@@ -0,0 +1,37 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_VERSION_H
#define LOVE_VERSION_H
namespace love
{
// Version stuff.
const int VERSION_MAJOR = 0;
const int VERSION_MINOR = 9;
const int VERSION_REV = 0;
const char *VERSION = "0.9.0";
const char *VERSION_COMPATIBILITY[] = { VERSION, 0 };
const char *VERSION_CODENAME = "Baby Inspector";
} // love
#endif // LOVE_VERSION_H
+66
View File
@@ -0,0 +1,66 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 "wrap_Data.h"
namespace love
{
Data *luax_checkdata(lua_State *L, int idx)
{
return luax_checktype<Data>(L, idx, "Data", DATA_T);
}
int w_Data_getString(lua_State *L)
{
Data *t = luax_checkdata(L, 1);
lua_pushlstring(L, (const char *) t->getData(), (size_t) t->getSize());
return 1;
}
int w_Data_getPointer(lua_State *L)
{
Data *t = luax_checkdata(L, 1);
lua_pushlightuserdata(L, t->getData());
return 1;
}
int w_Data_getSize(lua_State *L)
{
Data *t = luax_checkdata(L, 1);
lua_pushinteger(L, t->getSize());
return 1;
}
const luaL_Reg w_Data_functions[] =
{
{ "getString", w_Data_getString },
{ "getPointer", w_Data_getPointer },
{ "getSize", w_Data_getSize },
{ 0, 0 }
};
int w_Data_open(lua_State *L)
{
luax_register_type(L, "Data", w_Data_functions);
return 0;
}
} // love
+39
View File
@@ -0,0 +1,39 @@
/**
* Copyright (c) 2006-2013 LOVE Development Team
*
* 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 LOVE_WRAP_DATA_H
#define LOVE_WRAP_DATA_H
// LOVE
#include "runtime.h"
#include "Data.h"
namespace love
{
Data *luax_checkdata(lua_State *L, int idx);
int w_Data_getString(lua_State *L);
int w_Data_getPointer(lua_State *L);
int w_Data_getSize(lua_State *L);
int w_Data_open(lua_State *L);
} // love
#endif // LOVE_WRAP_DATA_H
+68
View File
@@ -0,0 +1,68 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 BOX2D_H
#define BOX2D_H
/**
\mainpage Box2D API Documentation
\section intro_sec Getting Started
For documentation please see http://box2d.org/documentation.html
For discussion please visit http://box2d.org/forum
*/
// These include files constitute the main Box2D API
#include <Box2D/Common/b2Settings.h>
#include <Box2D/Common/b2Draw.h>
#include <Box2D/Common/b2Timer.h>
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
#include <Box2D/Collision/Shapes/b2ChainShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
#include <Box2D/Collision/b2BroadPhase.h>
#include <Box2D/Collision/b2Distance.h>
#include <Box2D/Collision/b2DynamicTree.h>
#include <Box2D/Collision/b2TimeOfImpact.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <Box2D/Dynamics/b2WorldCallbacks.h>
#include <Box2D/Dynamics/b2TimeStep.h>
#include <Box2D/Dynamics/b2World.h>
#include <Box2D/Dynamics/Contacts/b2Contact.h>
#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
#include <Box2D/Dynamics/Joints/b2GearJoint.h>
#include <Box2D/Dynamics/Joints/b2MotorJoint.h>
#include <Box2D/Dynamics/Joints/b2MouseJoint.h>
#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
#include <Box2D/Dynamics/Joints/b2RopeJoint.h>
#include <Box2D/Dynamics/Joints/b2WeldJoint.h>
#include <Box2D/Dynamics/Joints/b2WheelJoint.h>
#endif
@@ -0,0 +1,190 @@
/*
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.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 <Box2D/Collision/Shapes/b2ChainShape.h>
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
#include <new>
#include <memory.h>
b2ChainShape::~b2ChainShape()
{
b2Free(m_vertices);
m_vertices = NULL;
m_count = 0;
}
void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
{
b2Assert(m_vertices == NULL && m_count == 0);
b2Assert(count >= 3);
for (int32 i = 1; i < count; ++i)
{
b2Vec2 v1 = vertices[i-1];
b2Vec2 v2 = vertices[i];
// If the code crashes here, it means your vertices are too close together.
b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
}
m_count = count + 1;
m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
memcpy(m_vertices, vertices, count * sizeof(b2Vec2));
m_vertices[count] = m_vertices[0];
m_prevVertex = m_vertices[m_count - 2];
m_nextVertex = m_vertices[1];
m_hasPrevVertex = true;
m_hasNextVertex = true;
}
void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)
{
b2Assert(m_vertices == NULL && m_count == 0);
b2Assert(count >= 2);
for (int32 i = 1; i < count; ++i)
{
b2Vec2 v1 = vertices[i-1];
b2Vec2 v2 = vertices[i];
// If the code crashes here, it means your vertices are too close together.
b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
}
m_count = count;
m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
m_hasPrevVertex = false;
m_hasNextVertex = false;
m_prevVertex.SetZero();
m_nextVertex.SetZero();
}
void b2ChainShape::SetPrevVertex(const b2Vec2& prevVertex)
{
m_prevVertex = prevVertex;
m_hasPrevVertex = true;
}
void b2ChainShape::SetNextVertex(const b2Vec2& nextVertex)
{
m_nextVertex = nextVertex;
m_hasNextVertex = true;
}
b2Shape* b2ChainShape::Clone(b2BlockAllocator* allocator) const
{
void* mem = allocator->Allocate(sizeof(b2ChainShape));
b2ChainShape* clone = new (mem) b2ChainShape;
clone->CreateChain(m_vertices, m_count);
clone->m_prevVertex = m_prevVertex;
clone->m_nextVertex = m_nextVertex;
clone->m_hasPrevVertex = m_hasPrevVertex;
clone->m_hasNextVertex = m_hasNextVertex;
return clone;
}
int32 b2ChainShape::GetChildCount() const
{
// edge count = vertex count - 1
return m_count - 1;
}
void b2ChainShape::GetChildEdge(b2EdgeShape* edge, int32 index) const
{
b2Assert(0 <= index && index < m_count - 1);
edge->m_type = b2Shape::e_edge;
edge->m_radius = m_radius;
edge->m_vertex1 = m_vertices[index + 0];
edge->m_vertex2 = m_vertices[index + 1];
if (index > 0)
{
edge->m_vertex0 = m_vertices[index - 1];
edge->m_hasVertex0 = true;
}
else
{
edge->m_vertex0 = m_prevVertex;
edge->m_hasVertex0 = m_hasPrevVertex;
}
if (index < m_count - 2)
{
edge->m_vertex3 = m_vertices[index + 2];
edge->m_hasVertex3 = true;
}
else
{
edge->m_vertex3 = m_nextVertex;
edge->m_hasVertex3 = m_hasNextVertex;
}
}
bool b2ChainShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
{
B2_NOT_USED(xf);
B2_NOT_USED(p);
return false;
}
bool b2ChainShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& xf, int32 childIndex) const
{
b2Assert(childIndex < m_count);
b2EdgeShape edgeShape;
int32 i1 = childIndex;
int32 i2 = childIndex + 1;
if (i2 == m_count)
{
i2 = 0;
}
edgeShape.m_vertex1 = m_vertices[i1];
edgeShape.m_vertex2 = m_vertices[i2];
return edgeShape.RayCast(output, input, xf, 0);
}
void b2ChainShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
{
b2Assert(childIndex < m_count);
int32 i1 = childIndex;
int32 i2 = childIndex + 1;
if (i2 == m_count)
{
i2 = 0;
}
b2Vec2 v1 = b2Mul(xf, m_vertices[i1]);
b2Vec2 v2 = b2Mul(xf, m_vertices[i2]);
aabb->lowerBound = b2Min(v1, v2);
aabb->upperBound = b2Max(v1, v2);
}
void b2ChainShape::ComputeMass(b2MassData* massData, float32 density) const
{
B2_NOT_USED(density);
massData->mass = 0.0f;
massData->center.SetZero();
massData->I = 0.0f;
}
@@ -0,0 +1,102 @@
/*
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.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 B2_CHAIN_SHAPE_H
#define B2_CHAIN_SHAPE_H
#include <Box2D/Collision/Shapes/b2Shape.h>
class b2EdgeShape;
/// A chain shape is a free form sequence of line segments.
/// The chain has two-sided collision, so you can use inside and outside collision.
/// Therefore, you may use any winding order.
/// Since there may be many vertices, they are allocated using b2Alloc.
/// Connectivity information is used to create smooth collisions.
/// WARNING: The chain will not collide properly if there are self-intersections.
class b2ChainShape : public b2Shape
{
public:
b2ChainShape();
/// The destructor frees the vertices using b2Free.
~b2ChainShape();
/// Create a loop. This automatically adjusts connectivity.
/// @param vertices an array of vertices, these are copied
/// @param count the vertex count
void CreateLoop(const b2Vec2* vertices, int32 count);
/// Create a chain with isolated end vertices.
/// @param vertices an array of vertices, these are copied
/// @param count the vertex count
void CreateChain(const b2Vec2* vertices, int32 count);
/// Establish connectivity to a vertex that precedes the first vertex.
/// Don't call this for loops.
void SetPrevVertex(const b2Vec2& prevVertex);
/// Establish connectivity to a vertex that follows the last vertex.
/// Don't call this for loops.
void SetNextVertex(const b2Vec2& nextVertex);
/// Implement b2Shape. Vertices are cloned using b2Alloc.
b2Shape* Clone(b2BlockAllocator* allocator) const;
/// @see b2Shape::GetChildCount
int32 GetChildCount() const;
/// Get a child edge.
void GetChildEdge(b2EdgeShape* edge, int32 index) const;
/// This always return false.
/// @see b2Shape::TestPoint
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
/// Implement b2Shape.
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& transform, int32 childIndex) const;
/// @see b2Shape::ComputeAABB
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
/// Chains have zero mass.
/// @see b2Shape::ComputeMass
void ComputeMass(b2MassData* massData, float32 density) const;
/// The vertices. Owned by this class.
b2Vec2* m_vertices;
/// The vertex count.
int32 m_count;
b2Vec2 m_prevVertex, m_nextVertex;
bool m_hasPrevVertex, m_hasNextVertex;
};
inline b2ChainShape::b2ChainShape()
{
m_type = e_chain;
m_radius = b2_polygonRadius;
m_vertices = NULL;
m_count = 0;
m_hasPrevVertex = false;
m_hasNextVertex = false;
}
#endif
@@ -0,0 +1,99 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Collision/Shapes/b2CircleShape.h>
#include <new>
b2Shape* b2CircleShape::Clone(b2BlockAllocator* allocator) const
{
void* mem = allocator->Allocate(sizeof(b2CircleShape));
b2CircleShape* clone = new (mem) b2CircleShape;
*clone = *this;
return clone;
}
int32 b2CircleShape::GetChildCount() const
{
return 1;
}
bool b2CircleShape::TestPoint(const b2Transform& transform, const b2Vec2& p) const
{
b2Vec2 center = transform.p + b2Mul(transform.q, m_p);
b2Vec2 d = p - center;
return b2Dot(d, d) <= m_radius * m_radius;
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& transform, int32 childIndex) const
{
B2_NOT_USED(childIndex);
b2Vec2 position = transform.p + b2Mul(transform.q, m_p);
b2Vec2 s = input.p1 - position;
float32 b = b2Dot(s, s) - m_radius * m_radius;
// Solve quadratic equation.
b2Vec2 r = input.p2 - input.p1;
float32 c = b2Dot(s, r);
float32 rr = b2Dot(r, r);
float32 sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < b2_epsilon)
{
return false;
}
// Find the point of intersection of the line with the circle.
float32 a = -(c + b2Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output->fraction = a;
output->normal = s + a * r;
output->normal.Normalize();
return true;
}
return false;
}
void b2CircleShape::ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const
{
B2_NOT_USED(childIndex);
b2Vec2 p = transform.p + b2Mul(transform.q, m_p);
aabb->lowerBound.Set(p.x - m_radius, p.y - m_radius);
aabb->upperBound.Set(p.x + m_radius, p.y + m_radius);
}
void b2CircleShape::ComputeMass(b2MassData* massData, float32 density) const
{
massData->mass = density * b2_pi * m_radius * m_radius;
massData->center = m_p;
// inertia about the local origin
massData->I = massData->mass * (0.5f * m_radius * m_radius + b2Dot(m_p, m_p));
}
@@ -0,0 +1,91 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_CIRCLE_SHAPE_H
#define B2_CIRCLE_SHAPE_H
#include <Box2D/Collision/Shapes/b2Shape.h>
/// A circle shape.
class b2CircleShape : public b2Shape
{
public:
b2CircleShape();
/// Implement b2Shape.
b2Shape* Clone(b2BlockAllocator* allocator) const;
/// @see b2Shape::GetChildCount
int32 GetChildCount() const;
/// Implement b2Shape.
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
/// Implement b2Shape.
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& transform, int32 childIndex) const;
/// @see b2Shape::ComputeAABB
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
/// @see b2Shape::ComputeMass
void ComputeMass(b2MassData* massData, float32 density) const;
/// Get the supporting vertex index in the given direction.
int32 GetSupport(const b2Vec2& d) const;
/// Get the supporting vertex in the given direction.
const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
/// Get the vertex count.
int32 GetVertexCount() const { return 1; }
/// Get a vertex by index. Used by b2Distance.
const b2Vec2& GetVertex(int32 index) const;
/// Position
b2Vec2 m_p;
};
inline b2CircleShape::b2CircleShape()
{
m_type = e_circle;
m_radius = 0.0f;
m_p.SetZero();
}
inline int32 b2CircleShape::GetSupport(const b2Vec2 &d) const
{
B2_NOT_USED(d);
return 0;
}
inline const b2Vec2& b2CircleShape::GetSupportVertex(const b2Vec2 &d) const
{
B2_NOT_USED(d);
return m_p;
}
inline const b2Vec2& b2CircleShape::GetVertex(int32 index) const
{
B2_NOT_USED(index);
b2Assert(index == 0);
return m_p;
}
#endif
@@ -0,0 +1,138 @@
/*
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.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 <Box2D/Collision/Shapes/b2EdgeShape.h>
#include <new>
void b2EdgeShape::Set(const b2Vec2& v1, const b2Vec2& v2)
{
m_vertex1 = v1;
m_vertex2 = v2;
m_hasVertex0 = false;
m_hasVertex3 = false;
}
b2Shape* b2EdgeShape::Clone(b2BlockAllocator* allocator) const
{
void* mem = allocator->Allocate(sizeof(b2EdgeShape));
b2EdgeShape* clone = new (mem) b2EdgeShape;
*clone = *this;
return clone;
}
int32 b2EdgeShape::GetChildCount() const
{
return 1;
}
bool b2EdgeShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
{
B2_NOT_USED(xf);
B2_NOT_USED(p);
return false;
}
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& xf, int32 childIndex) const
{
B2_NOT_USED(childIndex);
// Put the ray into the edge's frame of reference.
b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = m_vertex1;
b2Vec2 v2 = m_vertex2;
b2Vec2 e = v2 - v1;
b2Vec2 normal(e.y, -e.x);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float32 numerator = b2Dot(normal, v1 - p1);
float32 denominator = b2Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float32 t = numerator / denominator;
if (t < 0.0f || input.maxFraction < t)
{
return false;
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float32 rr = b2Dot(r, r);
if (rr == 0.0f)
{
return false;
}
float32 s = b2Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output->fraction = t;
if (numerator > 0.0f)
{
output->normal = -b2Mul(xf.q, normal);
}
else
{
output->normal = b2Mul(xf.q, normal);
}
return true;
}
void b2EdgeShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
{
B2_NOT_USED(childIndex);
b2Vec2 v1 = b2Mul(xf, m_vertex1);
b2Vec2 v2 = b2Mul(xf, m_vertex2);
b2Vec2 lower = b2Min(v1, v2);
b2Vec2 upper = b2Max(v1, v2);
b2Vec2 r(m_radius, m_radius);
aabb->lowerBound = lower - r;
aabb->upperBound = upper + r;
}
void b2EdgeShape::ComputeMass(b2MassData* massData, float32 density) const
{
B2_NOT_USED(density);
massData->mass = 0.0f;
massData->center = 0.5f * (m_vertex1 + m_vertex2);
massData->I = 0.0f;
}
@@ -0,0 +1,74 @@
/*
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.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 B2_EDGE_SHAPE_H
#define B2_EDGE_SHAPE_H
#include <Box2D/Collision/Shapes/b2Shape.h>
/// A line segment (edge) shape. These can be connected in chains or loops
/// to other edge shapes. The connectivity information is used to ensure
/// correct contact normals.
class b2EdgeShape : public b2Shape
{
public:
b2EdgeShape();
/// Set this as an isolated edge.
void Set(const b2Vec2& v1, const b2Vec2& v2);
/// Implement b2Shape.
b2Shape* Clone(b2BlockAllocator* allocator) const;
/// @see b2Shape::GetChildCount
int32 GetChildCount() const;
/// @see b2Shape::TestPoint
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
/// Implement b2Shape.
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& transform, int32 childIndex) const;
/// @see b2Shape::ComputeAABB
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
/// @see b2Shape::ComputeMass
void ComputeMass(b2MassData* massData, float32 density) const;
/// These are the edge vertices
b2Vec2 m_vertex1, m_vertex2;
/// Optional adjacent vertices. These are used for smooth collision.
b2Vec2 m_vertex0, m_vertex3;
bool m_hasVertex0, m_hasVertex3;
};
inline b2EdgeShape::b2EdgeShape()
{
m_type = e_edge;
m_radius = b2_polygonRadius;
m_vertex0.x = 0.0f;
m_vertex0.y = 0.0f;
m_vertex3.x = 0.0f;
m_vertex3.y = 0.0f;
m_hasVertex0 = false;
m_hasVertex3 = false;
}
#endif
@@ -0,0 +1,459 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Collision/Shapes/b2PolygonShape.h>
#include <new>
b2Shape* b2PolygonShape::Clone(b2BlockAllocator* allocator) const
{
void* mem = allocator->Allocate(sizeof(b2PolygonShape));
b2PolygonShape* clone = new (mem) b2PolygonShape;
*clone = *this;
return clone;
}
void b2PolygonShape::SetAsBox(float32 hx, float32 hy)
{
m_count = 4;
m_vertices[0].Set(-hx, -hy);
m_vertices[1].Set( hx, -hy);
m_vertices[2].Set( hx, hy);
m_vertices[3].Set(-hx, hy);
m_normals[0].Set(0.0f, -1.0f);
m_normals[1].Set(1.0f, 0.0f);
m_normals[2].Set(0.0f, 1.0f);
m_normals[3].Set(-1.0f, 0.0f);
m_centroid.SetZero();
}
void b2PolygonShape::SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle)
{
m_count = 4;
m_vertices[0].Set(-hx, -hy);
m_vertices[1].Set( hx, -hy);
m_vertices[2].Set( hx, hy);
m_vertices[3].Set(-hx, hy);
m_normals[0].Set(0.0f, -1.0f);
m_normals[1].Set(1.0f, 0.0f);
m_normals[2].Set(0.0f, 1.0f);
m_normals[3].Set(-1.0f, 0.0f);
m_centroid = center;
b2Transform xf;
xf.p = center;
xf.q.Set(angle);
// Transform vertices and normals.
for (int32 i = 0; i < m_count; ++i)
{
m_vertices[i] = b2Mul(xf, m_vertices[i]);
m_normals[i] = b2Mul(xf.q, m_normals[i]);
}
}
int32 b2PolygonShape::GetChildCount() const
{
return 1;
}
static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
{
b2Assert(count >= 3);
b2Vec2 c; c.Set(0.0f, 0.0f);
float32 area = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
b2Vec2 pRef(0.0f, 0.0f);
#if 0
// This code would put the reference point inside the polygon.
for (int32 i = 0; i < count; ++i)
{
pRef += vs[i];
}
pRef *= 1.0f / count;
#endif
const float32 inv3 = 1.0f / 3.0f;
for (int32 i = 0; i < count; ++i)
{
// Triangle vertices.
b2Vec2 p1 = pRef;
b2Vec2 p2 = vs[i];
b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
b2Vec2 e1 = p2 - p1;
b2Vec2 e2 = p3 - p1;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
c += triangleArea * inv3 * (p1 + p2 + p3);
}
// Centroid
b2Assert(area > b2_epsilon);
c *= 1.0f / area;
return c;
}
void b2PolygonShape::Set(const b2Vec2* vertices, int32 count)
{
b2Assert(3 <= count && count <= b2_maxPolygonVertices);
if (count < 3)
{
SetAsBox(1.0f, 1.0f);
return;
}
int32 n = b2Min(count, b2_maxPolygonVertices);
// Perform welding and copy vertices into local buffer.
b2Vec2 ps[b2_maxPolygonVertices];
int32 tempCount = 0;
for (int32 i = 0; i < n; ++i)
{
b2Vec2 v = vertices[i];
bool unique = true;
for (int32 j = 0; j < tempCount; ++j)
{
if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop)
{
unique = false;
break;
}
}
if (unique)
{
ps[tempCount++] = v;
}
}
n = tempCount;
if (n < 3)
{
// Polygon is degenerate.
b2Assert(false);
SetAsBox(1.0f, 1.0f);
return;
}
// Create the convex hull using the Gift wrapping algorithm
// http://en.wikipedia.org/wiki/Gift_wrapping_algorithm
// Find the right most point on the hull
int32 i0 = 0;
float32 x0 = ps[0].x;
for (int32 i = 1; i < n; ++i)
{
float32 x = ps[i].x;
if (x > x0 || (x == x0 && ps[i].y < ps[i0].y))
{
i0 = i;
x0 = x;
}
}
int32 hull[b2_maxPolygonVertices];
int32 m = 0;
int32 ih = i0;
for (;;)
{
hull[m] = ih;
int32 ie = 0;
for (int32 j = 1; j < n; ++j)
{
if (ie == ih)
{
ie = j;
continue;
}
b2Vec2 r = ps[ie] - ps[hull[m]];
b2Vec2 v = ps[j] - ps[hull[m]];
float32 c = b2Cross(r, v);
if (c < 0.0f)
{
ie = j;
}
// Collinearity check
if (c == 0.0f && v.LengthSquared() > r.LengthSquared())
{
ie = j;
}
}
++m;
ih = ie;
if (ie == i0)
{
break;
}
}
m_count = m;
// Copy vertices.
for (int32 i = 0; i < m; ++i)
{
m_vertices[i] = ps[hull[i]];
}
// Compute normals. Ensure the edges have non-zero length.
for (int32 i = 0; i < m; ++i)
{
int32 i1 = i;
int32 i2 = i + 1 < m ? i + 1 : 0;
b2Vec2 edge = m_vertices[i2] - m_vertices[i1];
b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon);
m_normals[i] = b2Cross(edge, 1.0f);
m_normals[i].Normalize();
}
// Compute the polygon centroid.
m_centroid = ComputeCentroid(m_vertices, m);
}
bool b2PolygonShape::TestPoint(const b2Transform& xf, const b2Vec2& p) const
{
b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
for (int32 i = 0; i < m_count; ++i)
{
float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
if (dot > 0.0f)
{
return false;
}
}
return true;
}
bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& xf, int32 childIndex) const
{
B2_NOT_USED(childIndex);
// Put the ray into the polygon's frame of reference.
b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
float32 lower = 0.0f, upper = input.maxFraction;
int32 index = -1;
for (int32 i = 0; i < m_count; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
float32 denominator = b2Dot(m_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return false;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return false;
}
}
b2Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output->fraction = lower;
output->normal = b2Mul(xf.q, m_normals[index]);
return true;
}
return false;
}
void b2PolygonShape::ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const
{
B2_NOT_USED(childIndex);
b2Vec2 lower = b2Mul(xf, m_vertices[0]);
b2Vec2 upper = lower;
for (int32 i = 1; i < m_count; ++i)
{
b2Vec2 v = b2Mul(xf, m_vertices[i]);
lower = b2Min(lower, v);
upper = b2Max(upper, v);
}
b2Vec2 r(m_radius, m_radius);
aabb->lowerBound = lower - r;
aabb->upperBound = upper + r;
}
void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
{
// Polygon mass, centroid, and inertia.
// Let rho be the polygon density in mass per unit area.
// Then:
// mass = rho * int(dA)
// centroid.x = (1/mass) * rho * int(x * dA)
// centroid.y = (1/mass) * rho * int(y * dA)
// I = rho * int((x*x + y*y) * dA)
//
// We can compute these integrals by summing all the integrals
// for each triangle of the polygon. To evaluate the integral
// for a single triangle, we make a change of variables to
// the (u,v) coordinates of the triangle:
// x = x0 + e1x * u + e2x * v
// y = y0 + e1y * u + e2y * v
// where 0 <= u && 0 <= v && u + v <= 1.
//
// We integrate u from [0,1-v] and then v from [0,1].
// We also need to use the Jacobian of the transformation:
// D = cross(e1, e2)
//
// Simplification: triangle centroid = (1/3) * (p1 + p2 + p3)
//
// The rest of the derivation is handled by computer algebra.
b2Assert(m_count >= 3);
b2Vec2 center; center.Set(0.0f, 0.0f);
float32 area = 0.0f;
float32 I = 0.0f;
// s is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
b2Vec2 s(0.0f, 0.0f);
// This code would put the reference point inside the polygon.
for (int32 i = 0; i < m_count; ++i)
{
s += m_vertices[i];
}
s *= 1.0f / m_count;
const float32 k_inv3 = 1.0f / 3.0f;
for (int32 i = 0; i < m_count; ++i)
{
// Triangle vertices.
b2Vec2 e1 = m_vertices[i] - s;
b2Vec2 e2 = i + 1 < m_count ? m_vertices[i+1] - s : m_vertices[0] - s;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (e1 + e2);
float32 ex1 = e1.x, ey1 = e1.y;
float32 ex2 = e2.x, ey2 = e2.y;
float32 intx2 = ex1*ex1 + ex2*ex1 + ex2*ex2;
float32 inty2 = ey1*ey1 + ey2*ey1 + ey2*ey2;
I += (0.25f * k_inv3 * D) * (intx2 + inty2);
}
// Total mass
massData->mass = density * area;
// Center of mass
b2Assert(area > b2_epsilon);
center *= 1.0f / area;
massData->center = center + s;
// Inertia tensor relative to the local origin (point s).
massData->I = density * I;
// Shift to center of mass then to original body origin.
massData->I += massData->mass * (b2Dot(massData->center, massData->center) - b2Dot(center, center));
}
bool b2PolygonShape::Validate() const
{
for (int32 i = 0; i < m_count; ++i)
{
int32 i1 = i;
int32 i2 = i < m_count - 1 ? i1 + 1 : 0;
b2Vec2 p = m_vertices[i1];
b2Vec2 e = m_vertices[i2] - p;
for (int32 j = 0; j < m_count; ++j)
{
if (j == i1 || j == i2)
{
continue;
}
b2Vec2 v = m_vertices[j] - p;
float32 c = b2Cross(e, v);
if (c < 0.0f)
{
return false;
}
}
}
return true;
}
@@ -0,0 +1,101 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_POLYGON_SHAPE_H
#define B2_POLYGON_SHAPE_H
#include <Box2D/Collision/Shapes/b2Shape.h>
/// A convex polygon. It is assumed that the interior of the polygon is to
/// the left of each edge.
/// Polygons have a maximum number of vertices equal to b2_maxPolygonVertices.
/// In most cases you should not need many vertices for a convex polygon.
class b2PolygonShape : public b2Shape
{
public:
b2PolygonShape();
/// Implement b2Shape.
b2Shape* Clone(b2BlockAllocator* allocator) const;
/// @see b2Shape::GetChildCount
int32 GetChildCount() const;
/// Create a convex hull from the given array of local points.
/// The count must be in the range [3, b2_maxPolygonVertices].
/// @warning the points may be re-ordered, even if they form a convex polygon
/// @warning collinear points are handled but not removed. Collinear points
/// may lead to poor stacking behavior.
void Set(const b2Vec2* points, int32 count);
/// Build vertices to represent an axis-aligned box centered on the local origin.
/// @param hx the half-width.
/// @param hy the half-height.
void SetAsBox(float32 hx, float32 hy);
/// Build vertices to represent an oriented box.
/// @param hx the half-width.
/// @param hy the half-height.
/// @param center the center of the box in local coordinates.
/// @param angle the rotation of the box in local coordinates.
void SetAsBox(float32 hx, float32 hy, const b2Vec2& center, float32 angle);
/// @see b2Shape::TestPoint
bool TestPoint(const b2Transform& transform, const b2Vec2& p) const;
/// Implement b2Shape.
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& transform, int32 childIndex) const;
/// @see b2Shape::ComputeAABB
void ComputeAABB(b2AABB* aabb, const b2Transform& transform, int32 childIndex) const;
/// @see b2Shape::ComputeMass
void ComputeMass(b2MassData* massData, float32 density) const;
/// Get the vertex count.
int32 GetVertexCount() const { return m_count; }
/// Get a vertex by index.
const b2Vec2& GetVertex(int32 index) const;
/// Validate convexity. This is a very time consuming operation.
/// @returns true if valid
bool Validate() const;
b2Vec2 m_centroid;
b2Vec2 m_vertices[b2_maxPolygonVertices];
b2Vec2 m_normals[b2_maxPolygonVertices];
int32 m_count;
};
inline b2PolygonShape::b2PolygonShape()
{
m_type = e_polygon;
m_radius = b2_polygonRadius;
m_count = 0;
m_centroid.SetZero();
}
inline const b2Vec2& b2PolygonShape::GetVertex(int32 index) const
{
b2Assert(0 <= index && index < m_count);
return m_vertices[index];
}
#endif
+101
View File
@@ -0,0 +1,101 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_SHAPE_H
#define B2_SHAPE_H
#include <Box2D/Common/b2BlockAllocator.h>
#include <Box2D/Common/b2Math.h>
#include <Box2D/Collision/b2Collision.h>
/// This holds the mass data computed for a shape.
struct b2MassData
{
/// The mass of the shape, usually in kilograms.
float32 mass;
/// The position of the shape's centroid relative to the shape's origin.
b2Vec2 center;
/// The rotational inertia of the shape about the local origin.
float32 I;
};
/// A shape is used for collision detection. You can create a shape however you like.
/// Shapes used for simulation in b2World are created automatically when a b2Fixture
/// is created. Shapes may encapsulate a one or more child shapes.
class b2Shape
{
public:
enum Type
{
e_circle = 0,
e_edge = 1,
e_polygon = 2,
e_chain = 3,
e_typeCount = 4
};
virtual ~b2Shape() {}
/// Clone the concrete shape using the provided allocator.
virtual b2Shape* Clone(b2BlockAllocator* allocator) const = 0;
/// Get the type of this shape. You can use this to down cast to the concrete shape.
/// @return the shape type.
Type GetType() const;
/// Get the number of child primitives.
virtual int32 GetChildCount() const = 0;
/// Test a point for containment in this shape. This only works for convex shapes.
/// @param xf the shape world transform.
/// @param p a point in world coordinates.
virtual bool TestPoint(const b2Transform& xf, const b2Vec2& p) const = 0;
/// Cast a ray against a child shape.
/// @param output the ray-cast results.
/// @param input the ray-cast input parameters.
/// @param transform the transform to be applied to the shape.
/// @param childIndex the child shape index
virtual bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& transform, int32 childIndex) const = 0;
/// Given a transform, compute the associated axis aligned bounding box for a child shape.
/// @param aabb returns the axis aligned box.
/// @param xf the world transform of the shape.
/// @param childIndex the child shape
virtual void ComputeAABB(b2AABB* aabb, const b2Transform& xf, int32 childIndex) const = 0;
/// Compute the mass properties of this shape using its dimensions and density.
/// The inertia tensor is computed about the local origin.
/// @param massData returns the mass data for this shape.
/// @param density the density in kilograms per meter squared.
virtual void ComputeMass(b2MassData* massData, float32 density) const = 0;
Type m_type;
float32 m_radius;
};
inline b2Shape::Type b2Shape::GetType() const
{
return m_type;
}
#endif
+120
View File
@@ -0,0 +1,120 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Collision/b2BroadPhase.h>
#include <memory.h>
b2BroadPhase::b2BroadPhase()
{
m_proxyCount = 0;
m_pairCapacity = 16;
m_pairCount = 0;
m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
m_moveCapacity = 16;
m_moveCount = 0;
m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
}
b2BroadPhase::~b2BroadPhase()
{
b2Free(m_moveBuffer);
b2Free(m_pairBuffer);
}
int32 b2BroadPhase::CreateProxy(const b2AABB& aabb, void* userData)
{
int32 proxyId = m_tree.CreateProxy(aabb, userData);
++m_proxyCount;
BufferMove(proxyId);
return proxyId;
}
void b2BroadPhase::DestroyProxy(int32 proxyId)
{
UnBufferMove(proxyId);
--m_proxyCount;
m_tree.DestroyProxy(proxyId);
}
void b2BroadPhase::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
{
bool buffer = m_tree.MoveProxy(proxyId, aabb, displacement);
if (buffer)
{
BufferMove(proxyId);
}
}
void b2BroadPhase::TouchProxy(int32 proxyId)
{
BufferMove(proxyId);
}
void b2BroadPhase::BufferMove(int32 proxyId)
{
if (m_moveCount == m_moveCapacity)
{
int32* oldBuffer = m_moveBuffer;
m_moveCapacity *= 2;
m_moveBuffer = (int32*)b2Alloc(m_moveCapacity * sizeof(int32));
memcpy(m_moveBuffer, oldBuffer, m_moveCount * sizeof(int32));
b2Free(oldBuffer);
}
m_moveBuffer[m_moveCount] = proxyId;
++m_moveCount;
}
void b2BroadPhase::UnBufferMove(int32 proxyId)
{
for (int32 i = 0; i < m_moveCount; ++i)
{
if (m_moveBuffer[i] == proxyId)
{
m_moveBuffer[i] = e_nullProxy;
}
}
}
// This is called from b2DynamicTree::Query when we are gathering pairs.
bool b2BroadPhase::QueryCallback(int32 proxyId)
{
// A proxy cannot form a pair with itself.
if (proxyId == m_queryProxyId)
{
return true;
}
// Grow the pair buffer as needed.
if (m_pairCount == m_pairCapacity)
{
b2Pair* oldBuffer = m_pairBuffer;
m_pairCapacity *= 2;
m_pairBuffer = (b2Pair*)b2Alloc(m_pairCapacity * sizeof(b2Pair));
memcpy(m_pairBuffer, oldBuffer, m_pairCount * sizeof(b2Pair));
b2Free(oldBuffer);
}
m_pairBuffer[m_pairCount].proxyIdA = b2Min(proxyId, m_queryProxyId);
m_pairBuffer[m_pairCount].proxyIdB = b2Max(proxyId, m_queryProxyId);
++m_pairCount;
return true;
}
+257
View File
@@ -0,0 +1,257 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_BROAD_PHASE_H
#define B2_BROAD_PHASE_H
#include <Box2D/Common/b2Settings.h>
#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/b2DynamicTree.h>
#include <algorithm>
struct b2Pair
{
int32 proxyIdA;
int32 proxyIdB;
};
/// The broad-phase is used for computing pairs and performing volume queries and ray casts.
/// This broad-phase does not persist pairs. Instead, this reports potentially new pairs.
/// It is up to the client to consume the new pairs and to track subsequent overlap.
class b2BroadPhase
{
public:
enum
{
e_nullProxy = -1
};
b2BroadPhase();
~b2BroadPhase();
/// Create a proxy with an initial AABB. Pairs are not reported until
/// UpdatePairs is called.
int32 CreateProxy(const b2AABB& aabb, void* userData);
/// Destroy a proxy. It is up to the client to remove any pairs.
void DestroyProxy(int32 proxyId);
/// Call MoveProxy as many times as you like, then when you are done
/// call UpdatePairs to finalized the proxy pairs (for your time step).
void MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement);
/// Call to trigger a re-processing of it's pairs on the next call to UpdatePairs.
void TouchProxy(int32 proxyId);
/// Get the fat AABB for a proxy.
const b2AABB& GetFatAABB(int32 proxyId) const;
/// Get user data from a proxy. Returns NULL if the id is invalid.
void* GetUserData(int32 proxyId) const;
/// Test overlap of fat AABBs.
bool TestOverlap(int32 proxyIdA, int32 proxyIdB) const;
/// Get the number of proxies.
int32 GetProxyCount() const;
/// Update the pairs. This results in pair callbacks. This can only add pairs.
template <typename T>
void UpdatePairs(T* callback);
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
template <typename T>
void Query(T* callback, const b2AABB& aabb) const;
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
/// @param callback a callback class that is called for each proxy that is hit by the ray.
template <typename T>
void RayCast(T* callback, const b2RayCastInput& input) const;
/// Get the height of the embedded tree.
int32 GetTreeHeight() const;
/// Get the balance of the embedded tree.
int32 GetTreeBalance() const;
/// Get the quality metric of the embedded tree.
float32 GetTreeQuality() const;
/// Shift the world origin. Useful for large worlds.
/// The shift formula is: position -= newOrigin
/// @param newOrigin the new origin with respect to the old origin
void ShiftOrigin(const b2Vec2& newOrigin);
private:
friend class b2DynamicTree;
void BufferMove(int32 proxyId);
void UnBufferMove(int32 proxyId);
bool QueryCallback(int32 proxyId);
b2DynamicTree m_tree;
int32 m_proxyCount;
int32* m_moveBuffer;
int32 m_moveCapacity;
int32 m_moveCount;
b2Pair* m_pairBuffer;
int32 m_pairCapacity;
int32 m_pairCount;
int32 m_queryProxyId;
};
/// This is used to sort pairs.
inline bool b2PairLessThan(const b2Pair& pair1, const b2Pair& pair2)
{
if (pair1.proxyIdA < pair2.proxyIdA)
{
return true;
}
if (pair1.proxyIdA == pair2.proxyIdA)
{
return pair1.proxyIdB < pair2.proxyIdB;
}
return false;
}
inline void* b2BroadPhase::GetUserData(int32 proxyId) const
{
return m_tree.GetUserData(proxyId);
}
inline bool b2BroadPhase::TestOverlap(int32 proxyIdA, int32 proxyIdB) const
{
const b2AABB& aabbA = m_tree.GetFatAABB(proxyIdA);
const b2AABB& aabbB = m_tree.GetFatAABB(proxyIdB);
return b2TestOverlap(aabbA, aabbB);
}
inline const b2AABB& b2BroadPhase::GetFatAABB(int32 proxyId) const
{
return m_tree.GetFatAABB(proxyId);
}
inline int32 b2BroadPhase::GetProxyCount() const
{
return m_proxyCount;
}
inline int32 b2BroadPhase::GetTreeHeight() const
{
return m_tree.GetHeight();
}
inline int32 b2BroadPhase::GetTreeBalance() const
{
return m_tree.GetMaxBalance();
}
inline float32 b2BroadPhase::GetTreeQuality() const
{
return m_tree.GetAreaRatio();
}
template <typename T>
void b2BroadPhase::UpdatePairs(T* callback)
{
// Reset pair buffer
m_pairCount = 0;
// Perform tree queries for all moving proxies.
for (int32 i = 0; i < m_moveCount; ++i)
{
m_queryProxyId = m_moveBuffer[i];
if (m_queryProxyId == e_nullProxy)
{
continue;
}
// We have to query the tree with the fat AABB so that
// we don't fail to create a pair that may touch later.
const b2AABB& fatAABB = m_tree.GetFatAABB(m_queryProxyId);
// Query tree, create pairs and add them pair buffer.
m_tree.Query(this, fatAABB);
}
// Reset move buffer
m_moveCount = 0;
// Sort the pair buffer to expose duplicates.
std::sort(m_pairBuffer, m_pairBuffer + m_pairCount, b2PairLessThan);
// Send the pairs back to the client.
int32 i = 0;
while (i < m_pairCount)
{
b2Pair* primaryPair = m_pairBuffer + i;
void* userDataA = m_tree.GetUserData(primaryPair->proxyIdA);
void* userDataB = m_tree.GetUserData(primaryPair->proxyIdB);
callback->AddPair(userDataA, userDataB);
++i;
// Skip any duplicate pairs.
while (i < m_pairCount)
{
b2Pair* pair = m_pairBuffer + i;
if (pair->proxyIdA != primaryPair->proxyIdA || pair->proxyIdB != primaryPair->proxyIdB)
{
break;
}
++i;
}
}
// Try to keep the tree balanced.
//m_tree.Rebalance(4);
}
template <typename T>
inline void b2BroadPhase::Query(T* callback, const b2AABB& aabb) const
{
m_tree.Query(callback, aabb);
}
template <typename T>
inline void b2BroadPhase::RayCast(T* callback, const b2RayCastInput& input) const
{
m_tree.RayCast(callback, input);
}
inline void b2BroadPhase::ShiftOrigin(const b2Vec2& newOrigin)
{
m_tree.ShiftOrigin(newOrigin);
}
#endif
+154
View File
@@ -0,0 +1,154 @@
/*
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.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 <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
void b2CollideCircles(
b2Manifold* manifold,
const b2CircleShape* circleA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB)
{
manifold->pointCount = 0;
b2Vec2 pA = b2Mul(xfA, circleA->m_p);
b2Vec2 pB = b2Mul(xfB, circleB->m_p);
b2Vec2 d = pB - pA;
float32 distSqr = b2Dot(d, d);
float32 rA = circleA->m_radius, rB = circleB->m_radius;
float32 radius = rA + rB;
if (distSqr > radius * radius)
{
return;
}
manifold->type = b2Manifold::e_circles;
manifold->localPoint = circleA->m_p;
manifold->localNormal.SetZero();
manifold->pointCount = 1;
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
}
void b2CollidePolygonAndCircle(
b2Manifold* manifold,
const b2PolygonShape* polygonA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB)
{
manifold->pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c = b2Mul(xfB, circleB->m_p);
b2Vec2 cLocal = b2MulT(xfA, c);
// Find the min separating edge.
int32 normalIndex = 0;
float32 separation = -b2_maxFloat;
float32 radius = polygonA->m_radius + circleB->m_radius;
int32 vertexCount = polygonA->m_count;
const b2Vec2* vertices = polygonA->m_vertices;
const b2Vec2* normals = polygonA->m_normals;
for (int32 i = 0; i < vertexCount; ++i)
{
float32 s = b2Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int32 vertIndex1 = normalIndex;
int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2_epsilon)
{
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = normals[normalIndex];
manifold->localPoint = 0.5f * (v1 + v2);
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
return;
}
// Compute barycentric coordinates
float32 u1 = b2Dot(cLocal - v1, v2 - v1);
float32 u2 = b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (b2DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = cLocal - v1;
manifold->localNormal.Normalize();
manifold->localPoint = v1;
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
}
else if (u2 <= 0.0f)
{
if (b2DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = cLocal - v2;
manifold->localNormal.Normalize();
manifold->localPoint = v2;
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
}
else
{
b2Vec2 faceCenter = 0.5f * (v1 + v2);
float32 separation = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = normals[vertIndex1];
manifold->localPoint = faceCenter;
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
}
}
+698
View File
@@ -0,0 +1,698 @@
/*
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.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 <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
void b2CollideEdgeAndCircle(b2Manifold* manifold,
const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB)
{
manifold->pointCount = 0;
// Compute circle in frame of edge
b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
b2Vec2 e = B - A;
// Barycentric coordinates
float32 u = b2Dot(e, B - Q);
float32 v = b2Dot(e, Q - A);
float32 radius = edgeA->m_radius + circleB->m_radius;
b2ContactFeature cf;
cf.indexB = 0;
cf.typeB = b2ContactFeature::e_vertex;
// Region A
if (v <= 0.0f)
{
b2Vec2 P = A;
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA->m_hasVertex0)
{
b2Vec2 A1 = edgeA->m_vertex0;
b2Vec2 B1 = A;
b2Vec2 e1 = B1 - A1;
float32 u1 = b2Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = b2ContactFeature::e_vertex;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_circles;
manifold->localNormal.SetZero();
manifold->localPoint = P;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
return;
}
// Region B
if (u <= 0.0f)
{
b2Vec2 P = B;
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA->m_hasVertex3)
{
b2Vec2 B2 = edgeA->m_vertex3;
b2Vec2 A2 = B;
b2Vec2 e2 = B2 - A2;
float32 v2 = b2Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = b2ContactFeature::e_vertex;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_circles;
manifold->localNormal.SetZero();
manifold->localPoint = P;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
return;
}
// Region AB
float32 den = b2Dot(e, e);
b2Assert(den > 0.0f);
b2Vec2 P = (1.0f / den) * (u * A + v * B);
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
b2Vec2 n(-e.y, e.x);
if (b2Dot(n, Q - A) < 0.0f)
{
n.Set(-n.x, -n.y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = b2ContactFeature::e_face;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = n;
manifold->localPoint = A;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
}
// This structure is used to keep track of the best separating axis.
struct b2EPAxis
{
enum Type
{
e_unknown,
e_edgeA,
e_edgeB
};
Type type;
int32 index;
float32 separation;
};
// This holds polygon B expressed in frame A.
struct b2TempPolygon
{
b2Vec2 vertices[b2_maxPolygonVertices];
b2Vec2 normals[b2_maxPolygonVertices];
int32 count;
};
// Reference face used for clipping
struct b2ReferenceFace
{
int32 i1, i2;
b2Vec2 v1, v2;
b2Vec2 normal;
b2Vec2 sideNormal1;
float32 sideOffset1;
b2Vec2 sideNormal2;
float32 sideOffset2;
};
// This class collides and edge and a polygon, taking into account edge adjacency.
struct b2EPCollider
{
void Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2PolygonShape* polygonB, const b2Transform& xfB);
b2EPAxis ComputeEdgeSeparation();
b2EPAxis ComputePolygonSeparation();
enum VertexType
{
e_isolated,
e_concave,
e_convex
};
b2TempPolygon m_polygonB;
b2Transform m_xf;
b2Vec2 m_centroidB;
b2Vec2 m_v0, m_v1, m_v2, m_v3;
b2Vec2 m_normal0, m_normal1, m_normal2;
b2Vec2 m_normal;
VertexType m_type1, m_type2;
b2Vec2 m_lowerLimit, m_upperLimit;
float32 m_radius;
bool m_front;
};
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2PolygonShape* polygonB, const b2Transform& xfB)
{
m_xf = b2MulT(xfA, xfB);
m_centroidB = b2Mul(m_xf, polygonB->m_centroid);
m_v0 = edgeA->m_vertex0;
m_v1 = edgeA->m_vertex1;
m_v2 = edgeA->m_vertex2;
m_v3 = edgeA->m_vertex3;
bool hasVertex0 = edgeA->m_hasVertex0;
bool hasVertex3 = edgeA->m_hasVertex3;
b2Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.y, -edge1.x);
float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1);
float32 offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
b2Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.y, -edge0.x);
convex1 = b2Cross(edge0, edge1) >= 0.0f;
offset0 = b2Dot(m_normal0, m_centroidB - m_v0);
}
// Is there a following edge?
if (hasVertex3)
{
b2Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.y, -edge2.x);
convex2 = b2Cross(edge1, edge2) > 0.0f;
offset2 = b2Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB.count = polygonB->m_count;
for (int32 i = 0; i < polygonB->m_count; ++i)
{
m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]);
}
m_radius = 2.0f * b2_polygonRadius;
manifold->pointCount = 0;
b2EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == b2EPAxis::e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
b2EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.type == b2EPAxis::e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2ClipVertex ie[2];
b2ReferenceFace rf;
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
manifold->type = b2Manifold::e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int32 bestIndex = 0;
float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]);
for (int32 i = 1; i < m_polygonB.count; ++i)
{
float32 value = b2Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int32 i1 = bestIndex;
int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = static_cast<uint8>(i1);
ie[0].id.cf.typeA = b2ContactFeature::e_face;
ie[0].id.cf.typeB = b2ContactFeature::e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = static_cast<uint8>(i2);
ie[1].id.cf.typeA = b2ContactFeature::e_face;
ie[1].id.cf.typeB = b2ContactFeature::e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold->type = b2Manifold::e_faceB;
ie[0].v = m_v1;
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
ie[0].id.cf.typeA = b2ContactFeature::e_vertex;
ie[0].id.cf.typeB = b2ContactFeature::e_face;
ie[1].v = m_v2;
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = static_cast<uint8>(primaryAxis.index);
ie[1].id.cf.typeA = b2ContactFeature::e_vertex;
ie[1].id.cf.typeB = b2ContactFeature::e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int32 np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
manifold->localNormal = rf.normal;
manifold->localPoint = rf.v1;
}
else
{
manifold->localNormal = polygonB->m_normals[rf.i1];
manifold->localPoint = polygonB->m_vertices[rf.i1];
}
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation;
separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
}
else
{
cp->localPoint = clipPoints2[i].v;
cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
++pointCount;
}
}
manifold->pointCount = pointCount;
}
b2EPAxis b2EPCollider::ComputeEdgeSeparation()
{
b2EPAxis axis;
axis.type = b2EPAxis::e_edgeA;
axis.index = m_front ? 0 : 1;
axis.separation = FLT_MAX;
for (int32 i = 0; i < m_polygonB.count; ++i)
{
float32 s = b2Dot(m_normal, m_polygonB.vertices[i] - m_v1);
if (s < axis.separation)
{
axis.separation = s;
}
}
return axis;
}
b2EPAxis b2EPCollider::ComputePolygonSeparation()
{
b2EPAxis axis;
axis.type = b2EPAxis::e_unknown;
axis.index = -1;
axis.separation = -FLT_MAX;
b2Vec2 perp(-m_normal.y, m_normal.x);
for (int32 i = 0; i < m_polygonB.count; ++i)
{
b2Vec2 n = -m_polygonB.normals[i];
float32 s1 = b2Dot(n, m_polygonB.vertices[i] - m_v1);
float32 s2 = b2Dot(n, m_polygonB.vertices[i] - m_v2);
float32 s = b2Min(s1, s2);
if (s > m_radius)
{
// No collision
axis.type = b2EPAxis::e_edgeB;
axis.index = i;
axis.separation = s;
return axis;
}
// Adjacency
if (b2Dot(n, perp) >= 0.0f)
{
if (b2Dot(n - m_upperLimit, m_normal) < -b2_angularSlop)
{
continue;
}
}
else
{
if (b2Dot(n - m_lowerLimit, m_normal) < -b2_angularSlop)
{
continue;
}
}
if (s > axis.separation)
{
axis.type = b2EPAxis::e_edgeB;
axis.index = i;
axis.separation = s;
}
}
return axis;
}
void b2CollideEdgeAndPolygon( b2Manifold* manifold,
const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2PolygonShape* polygonB, const b2Transform& xfB)
{
b2EPCollider collider;
collider.Collide(manifold, edgeA, xfA, polygonB, xfB);
}
+239
View File
@@ -0,0 +1,239 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static float32 b2FindMaxSeparation(int32* edgeIndex,
const b2PolygonShape* poly1, const b2Transform& xf1,
const b2PolygonShape* poly2, const b2Transform& xf2)
{
int32 count1 = poly1->m_count;
int32 count2 = poly2->m_count;
const b2Vec2* n1s = poly1->m_normals;
const b2Vec2* v1s = poly1->m_vertices;
const b2Vec2* v2s = poly2->m_vertices;
b2Transform xf = b2MulT(xf2, xf1);
int32 bestIndex = 0;
float32 maxSeparation = -b2_maxFloat;
for (int32 i = 0; i < count1; ++i)
{
// Get poly1 normal in frame2.
b2Vec2 n = b2Mul(xf.q, n1s[i]);
b2Vec2 v1 = b2Mul(xf, v1s[i]);
// Find deepest point for normal i.
float32 si = b2_maxFloat;
for (int32 j = 0; j < count2; ++j)
{
float32 sij = b2Dot(n, v2s[j] - v1);
if (sij < si)
{
si = sij;
}
}
if (si > maxSeparation)
{
maxSeparation = si;
bestIndex = i;
}
}
*edgeIndex = bestIndex;
return maxSeparation;
}
static void b2FindIncidentEdge(b2ClipVertex c[2],
const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
const b2PolygonShape* poly2, const b2Transform& xf2)
{
const b2Vec2* normals1 = poly1->m_normals;
int32 count2 = poly2->m_count;
const b2Vec2* vertices2 = poly2->m_vertices;
const b2Vec2* normals2 = poly2->m_normals;
b2Assert(0 <= edge1 && edge1 < poly1->m_count);
// Get the normal of the reference edge in poly2's frame.
b2Vec2 normal1 = b2MulT(xf2.q, b2Mul(xf1.q, normals1[edge1]));
// Find the incident edge on poly2.
int32 index = 0;
float32 minDot = b2_maxFloat;
for (int32 i = 0; i < count2; ++i)
{
float32 dot = b2Dot(normal1, normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int32 i1 = index;
int32 i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c[0].v = b2Mul(xf2, vertices2[i1]);
c[0].id.cf.indexA = (uint8)edge1;
c[0].id.cf.indexB = (uint8)i1;
c[0].id.cf.typeA = b2ContactFeature::e_face;
c[0].id.cf.typeB = b2ContactFeature::e_vertex;
c[1].v = b2Mul(xf2, vertices2[i2]);
c[1].id.cf.indexA = (uint8)edge1;
c[1].id.cf.indexB = (uint8)i2;
c[1].id.cf.typeA = b2ContactFeature::e_face;
c[1].id.cf.typeB = b2ContactFeature::e_vertex;
}
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
void b2CollidePolygons(b2Manifold* manifold,
const b2PolygonShape* polyA, const b2Transform& xfA,
const b2PolygonShape* polyB, const b2Transform& xfB)
{
manifold->pointCount = 0;
float32 totalRadius = polyA->m_radius + polyB->m_radius;
int32 edgeA = 0;
float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
return;
int32 edgeB = 0;
float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
return;
const b2PolygonShape* poly1; // reference polygon
const b2PolygonShape* poly2; // incident polygon
b2Transform xf1, xf2;
int32 edge1; // reference edge
uint8 flip;
const float32 k_tol = 0.1f * b2_linearSlop;
if (separationB > separationA + k_tol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold->type = b2Manifold::e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold->type = b2Manifold::e_faceA;
flip = 0;
}
b2ClipVertex incidentEdge[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int32 count1 = poly1->m_count;
const b2Vec2* vertices1 = poly1->m_vertices;
int32 iv1 = edge1;
int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent = v12 - v11;
localTangent.Normalize();
b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
b2Vec2 tangent = b2Mul(xf1.q, localTangent);
b2Vec2 normal = b2Cross(tangent, 1.0f);
v11 = b2Mul(xf1, v11);
v12 = b2Mul(xf1, v12);
// Face offset.
float32 frontOffset = b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
if (np < 2)
return;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold->localNormal = localNormal;
manifold->localPoint = planePoint;
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
cp->localPoint = b2MulT(xf2, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
if (flip)
{
// Swap features
b2ContactFeature cf = cp->id.cf;
cp->id.cf.indexA = cf.indexB;
cp->id.cf.indexB = cf.indexA;
cp->id.cf.typeA = cf.typeB;
cp->id.cf.typeB = cf.typeA;
}
++pointCount;
}
}
manifold->pointCount = pointCount;
}
+252
View File
@@ -0,0 +1,252 @@
/*
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.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 <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/b2Distance.h>
void b2WorldManifold::Initialize(const b2Manifold* manifold,
const b2Transform& xfA, float32 radiusA,
const b2Transform& xfB, float32 radiusB)
{
if (manifold->pointCount == 0)
{
return;
}
switch (manifold->type)
{
case b2Manifold::e_circles:
{
normal.Set(1.0f, 0.0f);
b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
{
normal = pointB - pointA;
normal.Normalize();
}
b2Vec2 cA = pointA + radiusA * normal;
b2Vec2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
separations[0] = b2Dot(cB - cA, normal);
}
break;
case b2Manifold::e_faceA:
{
normal = b2Mul(xfA.q, manifold->localNormal);
b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
for (int32 i = 0; i < manifold->pointCount; ++i)
{
b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
separations[i] = b2Dot(cB - cA, normal);
}
}
break;
case b2Manifold::e_faceB:
{
normal = b2Mul(xfB.q, manifold->localNormal);
b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
for (int32 i = 0; i < manifold->pointCount; ++i)
{
b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
separations[i] = b2Dot(cA - cB, normal);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
const b2Manifold* manifold1, const b2Manifold* manifold2)
{
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
state1[i] = b2_nullState;
state2[i] = b2_nullState;
}
// Detect persists and removes.
for (int32 i = 0; i < manifold1->pointCount; ++i)
{
b2ContactID id = manifold1->points[i].id;
state1[i] = b2_removeState;
for (int32 j = 0; j < manifold2->pointCount; ++j)
{
if (manifold2->points[j].id.key == id.key)
{
state1[i] = b2_persistState;
break;
}
}
}
// Detect persists and adds.
for (int32 i = 0; i < manifold2->pointCount; ++i)
{
b2ContactID id = manifold2->points[i].id;
state2[i] = b2_addState;
for (int32 j = 0; j < manifold1->pointCount; ++j)
{
if (manifold1->points[j].id.key == id.key)
{
state2[i] = b2_persistState;
break;
}
}
}
}
// From Real-time Collision Detection, p179.
bool b2AABB::RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const
{
float32 tmin = -b2_maxFloat;
float32 tmax = b2_maxFloat;
b2Vec2 p = input.p1;
b2Vec2 d = input.p2 - input.p1;
b2Vec2 absD = b2Abs(d);
b2Vec2 normal;
for (int32 i = 0; i < 2; ++i)
{
if (absD(i) < b2_epsilon)
{
// Parallel.
if (p(i) < lowerBound(i) || upperBound(i) < p(i))
{
return false;
}
}
else
{
float32 inv_d = 1.0f / d(i);
float32 t1 = (lowerBound(i) - p(i)) * inv_d;
float32 t2 = (upperBound(i) - p(i)) * inv_d;
// Sign of the normal vector.
float32 s = -1.0f;
if (t1 > t2)
{
b2Swap(t1, t2);
s = 1.0f;
}
// Push the min up
if (t1 > tmin)
{
normal.SetZero();
normal(i) = s;
tmin = t1;
}
// Pull the max down
tmax = b2Min(tmax, t2);
if (tmin > tmax)
{
return false;
}
}
}
// Does the ray start inside the box?
// Does the ray intersect beyond the max fraction?
if (tmin < 0.0f || input.maxFraction < tmin)
{
return false;
}
// Intersection.
output->fraction = tmin;
output->normal = normal;
return true;
}
// Sutherland-Hodgman clipping.
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
const b2Vec2& normal, float32 offset, int32 vertexIndexA)
{
// Start with no output points
int32 numOut = 0;
// Calculate the distance of end points to the line
float32 distance0 = b2Dot(normal, vIn[0].v) - offset;
float32 distance1 = b2Dot(normal, vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float32 interp = distance0 / (distance0 - distance1);
vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
// VertexA is hitting edgeB.
vOut[numOut].id.cf.indexA = static_cast<uint8>(vertexIndexA);
vOut[numOut].id.cf.indexB = vIn[0].id.cf.indexB;
vOut[numOut].id.cf.typeA = b2ContactFeature::e_vertex;
vOut[numOut].id.cf.typeB = b2ContactFeature::e_face;
++numOut;
}
return numOut;
}
bool b2TestOverlap( const b2Shape* shapeA, int32 indexA,
const b2Shape* shapeB, int32 indexB,
const b2Transform& xfA, const b2Transform& xfB)
{
b2DistanceInput input;
input.proxyA.Set(shapeA, indexA);
input.proxyB.Set(shapeB, indexB);
input.transformA = xfA;
input.transformB = xfB;
input.useRadii = true;
b2SimplexCache cache;
cache.count = 0;
b2DistanceOutput output;
b2Distance(&output, &cache, &input);
return output.distance < 10.0f * b2_epsilon;
}
+277
View File
@@ -0,0 +1,277 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_COLLISION_H
#define B2_COLLISION_H
#include <Box2D/Common/b2Math.h>
#include <limits.h>
/// @file
/// Structures and functions used for computing contact points, distance
/// queries, and TOI queries.
class b2Shape;
class b2CircleShape;
class b2EdgeShape;
class b2PolygonShape;
const uint8 b2_nullFeature = UCHAR_MAX;
/// The features that intersect to form the contact point
/// This must be 4 bytes or less.
struct b2ContactFeature
{
enum Type
{
e_vertex = 0,
e_face = 1
};
uint8 indexA; ///< Feature index on shapeA
uint8 indexB; ///< Feature index on shapeB
uint8 typeA; ///< The feature type on shapeA
uint8 typeB; ///< The feature type on shapeB
};
/// Contact ids to facilitate warm starting.
union b2ContactID
{
b2ContactFeature cf;
uint32 key; ///< Used to quickly compare contact ids.
};
/// A manifold point is a contact point belonging to a contact
/// manifold. It holds details related to the geometry and dynamics
/// of the contact points.
/// The local point usage depends on the manifold type:
/// -e_circles: the local center of circleB
/// -e_faceA: the local center of cirlceB or the clip point of polygonB
/// -e_faceB: the clip point of polygonA
/// This structure is stored across time steps, so we keep it small.
/// Note: the impulses are used for internal caching and may not
/// provide reliable contact forces, especially for high speed collisions.
struct b2ManifoldPoint
{
b2Vec2 localPoint; ///< usage depends on manifold type
float32 normalImpulse; ///< the non-penetration impulse
float32 tangentImpulse; ///< the friction impulse
b2ContactID id; ///< uniquely identifies a contact point between two shapes
};
/// A manifold for two touching convex shapes.
/// Box2D supports multiple types of contact:
/// - clip point versus plane with radius
/// - point versus point with radius (circles)
/// The local point usage depends on the manifold type:
/// -e_circles: the local center of circleA
/// -e_faceA: the center of faceA
/// -e_faceB: the center of faceB
/// Similarly the local normal usage:
/// -e_circles: not used
/// -e_faceA: the normal on polygonA
/// -e_faceB: the normal on polygonB
/// We store contacts in this way so that position correction can
/// account for movement, which is critical for continuous physics.
/// All contact scenarios must be expressed in one of these types.
/// This structure is stored across time steps, so we keep it small.
struct b2Manifold
{
enum Type
{
e_circles,
e_faceA,
e_faceB
};
b2ManifoldPoint points[b2_maxManifoldPoints]; ///< the points of contact
b2Vec2 localNormal; ///< not use for Type::e_points
b2Vec2 localPoint; ///< usage depends on manifold type
Type type;
int32 pointCount; ///< the number of manifold points
};
/// This is used to compute the current state of a contact manifold.
struct b2WorldManifold
{
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the shapes
/// that generated the manifold.
void Initialize(const b2Manifold* manifold,
const b2Transform& xfA, float32 radiusA,
const b2Transform& xfB, float32 radiusB);
b2Vec2 normal; ///< world vector pointing from A to B
b2Vec2 points[b2_maxManifoldPoints]; ///< world contact point (point of intersection)
float32 separations[b2_maxManifoldPoints]; ///< a negative value indicates overlap, in meters
};
/// This is used for determining the state of contact points.
enum b2PointState
{
b2_nullState, ///< point does not exist
b2_addState, ///< point was added in the update
b2_persistState, ///< point persisted across the update
b2_removeState ///< point was removed in the update
};
/// Compute the point states given two manifolds. The states pertain to the transition from manifold1
/// to manifold2. So state1 is either persist or remove while state2 is either add or persist.
void b2GetPointStates(b2PointState state1[b2_maxManifoldPoints], b2PointState state2[b2_maxManifoldPoints],
const b2Manifold* manifold1, const b2Manifold* manifold2);
/// Used for computing contact manifolds.
struct b2ClipVertex
{
b2Vec2 v;
b2ContactID id;
};
/// Ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
struct b2RayCastInput
{
b2Vec2 p1, p2;
float32 maxFraction;
};
/// Ray-cast output data. The ray hits at p1 + fraction * (p2 - p1), where p1 and p2
/// come from b2RayCastInput.
struct b2RayCastOutput
{
b2Vec2 normal;
float32 fraction;
};
/// An axis aligned bounding box.
struct b2AABB
{
/// Verify that the bounds are sorted.
bool IsValid() const;
/// Get the center of the AABB.
b2Vec2 GetCenter() const
{
return 0.5f * (lowerBound + upperBound);
}
/// Get the extents of the AABB (half-widths).
b2Vec2 GetExtents() const
{
return 0.5f * (upperBound - lowerBound);
}
/// Get the perimeter length
float32 GetPerimeter() const
{
float32 wx = upperBound.x - lowerBound.x;
float32 wy = upperBound.y - lowerBound.y;
return 2.0f * (wx + wy);
}
/// Combine an AABB into this one.
void Combine(const b2AABB& aabb)
{
lowerBound = b2Min(lowerBound, aabb.lowerBound);
upperBound = b2Max(upperBound, aabb.upperBound);
}
/// Combine two AABBs into this one.
void Combine(const b2AABB& aabb1, const b2AABB& aabb2)
{
lowerBound = b2Min(aabb1.lowerBound, aabb2.lowerBound);
upperBound = b2Max(aabb1.upperBound, aabb2.upperBound);
}
/// Does this aabb contain the provided AABB.
bool Contains(const b2AABB& aabb) const
{
bool result = true;
result = result && lowerBound.x <= aabb.lowerBound.x;
result = result && lowerBound.y <= aabb.lowerBound.y;
result = result && aabb.upperBound.x <= upperBound.x;
result = result && aabb.upperBound.y <= upperBound.y;
return result;
}
bool RayCast(b2RayCastOutput* output, const b2RayCastInput& input) const;
b2Vec2 lowerBound; ///< the lower vertex
b2Vec2 upperBound; ///< the upper vertex
};
/// Compute the collision manifold between two circles.
void b2CollideCircles(b2Manifold* manifold,
const b2CircleShape* circleA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB);
/// Compute the collision manifold between a polygon and a circle.
void b2CollidePolygonAndCircle(b2Manifold* manifold,
const b2PolygonShape* polygonA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB);
/// Compute the collision manifold between two polygons.
void b2CollidePolygons(b2Manifold* manifold,
const b2PolygonShape* polygonA, const b2Transform& xfA,
const b2PolygonShape* polygonB, const b2Transform& xfB);
/// Compute the collision manifold between an edge and a circle.
void b2CollideEdgeAndCircle(b2Manifold* manifold,
const b2EdgeShape* polygonA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB);
/// Compute the collision manifold between an edge and a circle.
void b2CollideEdgeAndPolygon(b2Manifold* manifold,
const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2PolygonShape* circleB, const b2Transform& xfB);
/// Clipping for contact manifolds.
int32 b2ClipSegmentToLine(b2ClipVertex vOut[2], const b2ClipVertex vIn[2],
const b2Vec2& normal, float32 offset, int32 vertexIndexA);
/// Determine if two generic shapes overlap.
bool b2TestOverlap( const b2Shape* shapeA, int32 indexA,
const b2Shape* shapeB, int32 indexB,
const b2Transform& xfA, const b2Transform& xfB);
// ---------------- Inline Functions ------------------------------------------
inline bool b2AABB::IsValid() const
{
b2Vec2 d = upperBound - lowerBound;
bool valid = d.x >= 0.0f && d.y >= 0.0f;
valid = valid && lowerBound.IsValid() && upperBound.IsValid();
return valid;
}
inline bool b2TestOverlap(const b2AABB& a, const b2AABB& b)
{
b2Vec2 d1, d2;
d1 = b.lowerBound - a.upperBound;
d2 = a.lowerBound - b.upperBound;
if (d1.x > 0.0f || d1.y > 0.0f)
return false;
if (d2.x > 0.0f || d2.y > 0.0f)
return false;
return true;
}
#endif
+603
View File
@@ -0,0 +1,603 @@
/*
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.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 <Box2D/Collision/b2Distance.h>
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
#include <Box2D/Collision/Shapes/b2ChainShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
// GJK using Voronoi regions (Christer Ericson) and Barycentric coordinates.
int32 b2_gjkCalls, b2_gjkIters, b2_gjkMaxIters;
void b2DistanceProxy::Set(const b2Shape* shape, int32 index)
{
switch (shape->GetType())
{
case b2Shape::e_circle:
{
const b2CircleShape* circle = static_cast<const b2CircleShape*>(shape);
m_vertices = &circle->m_p;
m_count = 1;
m_radius = circle->m_radius;
}
break;
case b2Shape::e_polygon:
{
const b2PolygonShape* polygon = static_cast<const b2PolygonShape*>(shape);
m_vertices = polygon->m_vertices;
m_count = polygon->m_count;
m_radius = polygon->m_radius;
}
break;
case b2Shape::e_chain:
{
const b2ChainShape* chain = static_cast<const b2ChainShape*>(shape);
b2Assert(0 <= index && index < chain->m_count);
m_buffer[0] = chain->m_vertices[index];
if (index + 1 < chain->m_count)
{
m_buffer[1] = chain->m_vertices[index + 1];
}
else
{
m_buffer[1] = chain->m_vertices[0];
}
m_vertices = m_buffer;
m_count = 2;
m_radius = chain->m_radius;
}
break;
case b2Shape::e_edge:
{
const b2EdgeShape* edge = static_cast<const b2EdgeShape*>(shape);
m_vertices = &edge->m_vertex1;
m_count = 2;
m_radius = edge->m_radius;
}
break;
default:
b2Assert(false);
}
}
struct b2SimplexVertex
{
b2Vec2 wA; // support point in proxyA
b2Vec2 wB; // support point in proxyB
b2Vec2 w; // wB - wA
float32 a; // barycentric coordinate for closest point
int32 indexA; // wA index
int32 indexB; // wB index
};
struct b2Simplex
{
void ReadCache( const b2SimplexCache* cache,
const b2DistanceProxy* proxyA, const b2Transform& transformA,
const b2DistanceProxy* proxyB, const b2Transform& transformB)
{
b2Assert(cache->count <= 3);
// Copy data from cache.
m_count = cache->count;
b2SimplexVertex* vertices = &m_v1;
for (int32 i = 0; i < m_count; ++i)
{
b2SimplexVertex* v = vertices + i;
v->indexA = cache->indexA[i];
v->indexB = cache->indexB[i];
b2Vec2 wALocal = proxyA->GetVertex(v->indexA);
b2Vec2 wBLocal = proxyB->GetVertex(v->indexB);
v->wA = b2Mul(transformA, wALocal);
v->wB = b2Mul(transformB, wBLocal);
v->w = v->wB - v->wA;
v->a = 0.0f;
}
// Compute the new simplex metric, if it is substantially different than
// old metric then flush the simplex.
if (m_count > 1)
{
float32 metric1 = cache->metric;
float32 metric2 = GetMetric();
if (metric2 < 0.5f * metric1 || 2.0f * metric1 < metric2 || metric2 < b2_epsilon)
{
// Reset the simplex.
m_count = 0;
}
}
// If the cache is empty or invalid ...
if (m_count == 0)
{
b2SimplexVertex* v = vertices + 0;
v->indexA = 0;
v->indexB = 0;
b2Vec2 wALocal = proxyA->GetVertex(0);
b2Vec2 wBLocal = proxyB->GetVertex(0);
v->wA = b2Mul(transformA, wALocal);
v->wB = b2Mul(transformB, wBLocal);
v->w = v->wB - v->wA;
v->a = 1.0f;
m_count = 1;
}
}
void WriteCache(b2SimplexCache* cache) const
{
cache->metric = GetMetric();
cache->count = uint16(m_count);
const b2SimplexVertex* vertices = &m_v1;
for (int32 i = 0; i < m_count; ++i)
{
cache->indexA[i] = uint8(vertices[i].indexA);
cache->indexB[i] = uint8(vertices[i].indexB);
}
}
b2Vec2 GetSearchDirection() const
{
switch (m_count)
{
case 1:
return -m_v1.w;
case 2:
{
b2Vec2 e12 = m_v2.w - m_v1.w;
float32 sgn = b2Cross(e12, -m_v1.w);
if (sgn > 0.0f)
{
// Origin is left of e12.
return b2Cross(1.0f, e12);
}
else
{
// Origin is right of e12.
return b2Cross(e12, 1.0f);
}
}
default:
b2Assert(false);
return b2Vec2_zero;
}
}
b2Vec2 GetClosestPoint() const
{
switch (m_count)
{
case 0:
b2Assert(false);
return b2Vec2_zero;
case 1:
return m_v1.w;
case 2:
return m_v1.a * m_v1.w + m_v2.a * m_v2.w;
case 3:
return b2Vec2_zero;
default:
b2Assert(false);
return b2Vec2_zero;
}
}
void GetWitnessPoints(b2Vec2* pA, b2Vec2* pB) const
{
switch (m_count)
{
case 0:
b2Assert(false);
break;
case 1:
*pA = m_v1.wA;
*pB = m_v1.wB;
break;
case 2:
*pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA;
*pB = m_v1.a * m_v1.wB + m_v2.a * m_v2.wB;
break;
case 3:
*pA = m_v1.a * m_v1.wA + m_v2.a * m_v2.wA + m_v3.a * m_v3.wA;
*pB = *pA;
break;
default:
b2Assert(false);
break;
}
}
float32 GetMetric() const
{
switch (m_count)
{
case 0:
b2Assert(false);
return 0.0f;
case 1:
return 0.0f;
case 2:
return b2Distance(m_v1.w, m_v2.w);
case 3:
return b2Cross(m_v2.w - m_v1.w, m_v3.w - m_v1.w);
default:
b2Assert(false);
return 0.0f;
}
}
void Solve2();
void Solve3();
b2SimplexVertex m_v1, m_v2, m_v3;
int32 m_count;
};
// Solve a line segment using barycentric coordinates.
//
// p = a1 * w1 + a2 * w2
// a1 + a2 = 1
//
// The vector from the origin to the closest point on the line is
// perpendicular to the line.
// e12 = w2 - w1
// dot(p, e) = 0
// a1 * dot(w1, e) + a2 * dot(w2, e) = 0
//
// 2-by-2 linear system
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
//
// Define
// d12_1 = dot(w2, e12)
// d12_2 = -dot(w1, e12)
// d12 = d12_1 + d12_2
//
// Solution
// a1 = d12_1 / d12
// a2 = d12_2 / d12
void b2Simplex::Solve2()
{
b2Vec2 w1 = m_v1.w;
b2Vec2 w2 = m_v2.w;
b2Vec2 e12 = w2 - w1;
// w1 region
float32 d12_2 = -b2Dot(w1, e12);
if (d12_2 <= 0.0f)
{
// a2 <= 0, so we clamp it to 0
m_v1.a = 1.0f;
m_count = 1;
return;
}
// w2 region
float32 d12_1 = b2Dot(w2, e12);
if (d12_1 <= 0.0f)
{
// a1 <= 0, so we clamp it to 0
m_v2.a = 1.0f;
m_count = 1;
m_v1 = m_v2;
return;
}
// Must be in e12 region.
float32 inv_d12 = 1.0f / (d12_1 + d12_2);
m_v1.a = d12_1 * inv_d12;
m_v2.a = d12_2 * inv_d12;
m_count = 2;
}
// Possible regions:
// - points[2]
// - edge points[0]-points[2]
// - edge points[1]-points[2]
// - inside the triangle
void b2Simplex::Solve3()
{
b2Vec2 w1 = m_v1.w;
b2Vec2 w2 = m_v2.w;
b2Vec2 w3 = m_v3.w;
// Edge12
// [1 1 ][a1] = [1]
// [w1.e12 w2.e12][a2] = [0]
// a3 = 0
b2Vec2 e12 = w2 - w1;
float32 w1e12 = b2Dot(w1, e12);
float32 w2e12 = b2Dot(w2, e12);
float32 d12_1 = w2e12;
float32 d12_2 = -w1e12;
// Edge13
// [1 1 ][a1] = [1]
// [w1.e13 w3.e13][a3] = [0]
// a2 = 0
b2Vec2 e13 = w3 - w1;
float32 w1e13 = b2Dot(w1, e13);
float32 w3e13 = b2Dot(w3, e13);
float32 d13_1 = w3e13;
float32 d13_2 = -w1e13;
// Edge23
// [1 1 ][a2] = [1]
// [w2.e23 w3.e23][a3] = [0]
// a1 = 0
b2Vec2 e23 = w3 - w2;
float32 w2e23 = b2Dot(w2, e23);
float32 w3e23 = b2Dot(w3, e23);
float32 d23_1 = w3e23;
float32 d23_2 = -w2e23;
// Triangle123
float32 n123 = b2Cross(e12, e13);
float32 d123_1 = n123 * b2Cross(w2, w3);
float32 d123_2 = n123 * b2Cross(w3, w1);
float32 d123_3 = n123 * b2Cross(w1, w2);
// w1 region
if (d12_2 <= 0.0f && d13_2 <= 0.0f)
{
m_v1.a = 1.0f;
m_count = 1;
return;
}
// e12
if (d12_1 > 0.0f && d12_2 > 0.0f && d123_3 <= 0.0f)
{
float32 inv_d12 = 1.0f / (d12_1 + d12_2);
m_v1.a = d12_1 * inv_d12;
m_v2.a = d12_2 * inv_d12;
m_count = 2;
return;
}
// e13
if (d13_1 > 0.0f && d13_2 > 0.0f && d123_2 <= 0.0f)
{
float32 inv_d13 = 1.0f / (d13_1 + d13_2);
m_v1.a = d13_1 * inv_d13;
m_v3.a = d13_2 * inv_d13;
m_count = 2;
m_v2 = m_v3;
return;
}
// w2 region
if (d12_1 <= 0.0f && d23_2 <= 0.0f)
{
m_v2.a = 1.0f;
m_count = 1;
m_v1 = m_v2;
return;
}
// w3 region
if (d13_1 <= 0.0f && d23_1 <= 0.0f)
{
m_v3.a = 1.0f;
m_count = 1;
m_v1 = m_v3;
return;
}
// e23
if (d23_1 > 0.0f && d23_2 > 0.0f && d123_1 <= 0.0f)
{
float32 inv_d23 = 1.0f / (d23_1 + d23_2);
m_v2.a = d23_1 * inv_d23;
m_v3.a = d23_2 * inv_d23;
m_count = 2;
m_v1 = m_v3;
return;
}
// Must be in triangle123
float32 inv_d123 = 1.0f / (d123_1 + d123_2 + d123_3);
m_v1.a = d123_1 * inv_d123;
m_v2.a = d123_2 * inv_d123;
m_v3.a = d123_3 * inv_d123;
m_count = 3;
}
void b2Distance(b2DistanceOutput* output,
b2SimplexCache* cache,
const b2DistanceInput* input)
{
++b2_gjkCalls;
const b2DistanceProxy* proxyA = &input->proxyA;
const b2DistanceProxy* proxyB = &input->proxyB;
b2Transform transformA = input->transformA;
b2Transform transformB = input->transformB;
// Initialize the simplex.
b2Simplex simplex;
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
b2SimplexVertex* vertices = &simplex.m_v1;
const int32 k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int32 saveA[3], saveB[3];
int32 saveCount = 0;
float32 distanceSqr1 = b2_maxFloat;
float32 distanceSqr2 = distanceSqr1;
// Main iteration loop.
int32 iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int32 i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
b2Assert(false);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.m_count == 3)
{
break;
}
// Compute closest point.
b2Vec2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < b2_epsilon * b2_epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
b2SimplexVertex* vertex = vertices + simplex.m_count;
vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
b2Vec2 wBLocal;
vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
// Iteration count is equated to the number of support point calls.
++iter;
++b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int32 i = 0; i < saveCount; ++i)
{
if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.m_count;
}
b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(&output->pointA, &output->pointB);
output->distance = b2Distance(output->pointA, output->pointB);
output->iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
// Apply radii if requested.
if (input->useRadii)
{
float32 rA = proxyA->m_radius;
float32 rB = proxyB->m_radius;
if (output->distance > rA + rB && output->distance > b2_epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output->distance -= rA + rB;
b2Vec2 normal = output->pointB - output->pointA;
normal.Normalize();
output->pointA += rA * normal;
output->pointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
b2Vec2 p = 0.5f * (output->pointA + output->pointB);
output->pointA = p;
output->pointB = p;
output->distance = 0.0f;
}
}
}
+141
View File
@@ -0,0 +1,141 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_DISTANCE_H
#define B2_DISTANCE_H
#include <Box2D/Common/b2Math.h>
class b2Shape;
/// A distance proxy is used by the GJK algorithm.
/// It encapsulates any shape.
struct b2DistanceProxy
{
b2DistanceProxy() : m_vertices(NULL), m_count(0), m_radius(0.0f) {}
/// Initialize the proxy using the given shape. The shape
/// must remain in scope while the proxy is in use.
void Set(const b2Shape* shape, int32 index);
/// Get the supporting vertex index in the given direction.
int32 GetSupport(const b2Vec2& d) const;
/// Get the supporting vertex in the given direction.
const b2Vec2& GetSupportVertex(const b2Vec2& d) const;
/// Get the vertex count.
int32 GetVertexCount() const;
/// Get a vertex by index. Used by b2Distance.
const b2Vec2& GetVertex(int32 index) const;
b2Vec2 m_buffer[2];
const b2Vec2* m_vertices;
int32 m_count;
float32 m_radius;
};
/// Used to warm start b2Distance.
/// Set count to zero on first call.
struct b2SimplexCache
{
float32 metric; ///< length or area
uint16 count;
uint8 indexA[3]; ///< vertices on shape A
uint8 indexB[3]; ///< vertices on shape B
};
/// Input for b2Distance.
/// You have to option to use the shape radii
/// in the computation. Even
struct b2DistanceInput
{
b2DistanceProxy proxyA;
b2DistanceProxy proxyB;
b2Transform transformA;
b2Transform transformB;
bool useRadii;
};
/// Output for b2Distance.
struct b2DistanceOutput
{
b2Vec2 pointA; ///< closest point on shapeA
b2Vec2 pointB; ///< closest point on shapeB
float32 distance;
int32 iterations; ///< number of GJK iterations used
};
/// Compute the closest points between two shapes. Supports any combination of:
/// b2CircleShape, b2PolygonShape, b2EdgeShape. The simplex cache is input/output.
/// On the first call set b2SimplexCache.count to zero.
void b2Distance(b2DistanceOutput* output,
b2SimplexCache* cache,
const b2DistanceInput* input);
//////////////////////////////////////////////////////////////////////////
inline int32 b2DistanceProxy::GetVertexCount() const
{
return m_count;
}
inline const b2Vec2& b2DistanceProxy::GetVertex(int32 index) const
{
b2Assert(0 <= index && index < m_count);
return m_vertices[index];
}
inline int32 b2DistanceProxy::GetSupport(const b2Vec2& d) const
{
int32 bestIndex = 0;
float32 bestValue = b2Dot(m_vertices[0], d);
for (int32 i = 1; i < m_count; ++i)
{
float32 value = b2Dot(m_vertices[i], d);
if (value > bestValue)
{
bestIndex = i;
bestValue = value;
}
}
return bestIndex;
}
inline const b2Vec2& b2DistanceProxy::GetSupportVertex(const b2Vec2& d) const
{
int32 bestIndex = 0;
float32 bestValue = b2Dot(m_vertices[0], d);
for (int32 i = 1; i < m_count; ++i)
{
float32 value = b2Dot(m_vertices[i], d);
if (value > bestValue)
{
bestIndex = i;
bestValue = value;
}
}
return m_vertices[bestIndex];
}
#endif
+778
View File
@@ -0,0 +1,778 @@
/*
* Copyright (c) 2009 Erin Catto http://www.box2d.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 <Box2D/Collision/b2DynamicTree.h>
#include <memory.h>
b2DynamicTree::b2DynamicTree()
{
m_root = b2_nullNode;
m_nodeCapacity = 16;
m_nodeCount = 0;
m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
memset(m_nodes, 0, m_nodeCapacity * sizeof(b2TreeNode));
// Build a linked list for the free list.
for (int32 i = 0; i < m_nodeCapacity - 1; ++i)
{
m_nodes[i].next = i + 1;
m_nodes[i].height = -1;
}
m_nodes[m_nodeCapacity-1].next = b2_nullNode;
m_nodes[m_nodeCapacity-1].height = -1;
m_freeList = 0;
m_path = 0;
m_insertionCount = 0;
}
b2DynamicTree::~b2DynamicTree()
{
// This frees the entire tree in one shot.
b2Free(m_nodes);
}
// Allocate a node from the pool. Grow the pool if necessary.
int32 b2DynamicTree::AllocateNode()
{
// Expand the node pool as needed.
if (m_freeList == b2_nullNode)
{
b2Assert(m_nodeCount == m_nodeCapacity);
// The free list is empty. Rebuild a bigger pool.
b2TreeNode* oldNodes = m_nodes;
m_nodeCapacity *= 2;
m_nodes = (b2TreeNode*)b2Alloc(m_nodeCapacity * sizeof(b2TreeNode));
memcpy(m_nodes, oldNodes, m_nodeCount * sizeof(b2TreeNode));
b2Free(oldNodes);
// Build a linked list for the free list. The parent
// pointer becomes the "next" pointer.
for (int32 i = m_nodeCount; i < m_nodeCapacity - 1; ++i)
{
m_nodes[i].next = i + 1;
m_nodes[i].height = -1;
}
m_nodes[m_nodeCapacity-1].next = b2_nullNode;
m_nodes[m_nodeCapacity-1].height = -1;
m_freeList = m_nodeCount;
}
// Peel a node off the free list.
int32 nodeId = m_freeList;
m_freeList = m_nodes[nodeId].next;
m_nodes[nodeId].parent = b2_nullNode;
m_nodes[nodeId].child1 = b2_nullNode;
m_nodes[nodeId].child2 = b2_nullNode;
m_nodes[nodeId].height = 0;
m_nodes[nodeId].userData = NULL;
++m_nodeCount;
return nodeId;
}
// Return a node to the pool.
void b2DynamicTree::FreeNode(int32 nodeId)
{
b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
b2Assert(0 < m_nodeCount);
m_nodes[nodeId].next = m_freeList;
m_nodes[nodeId].height = -1;
m_freeList = nodeId;
--m_nodeCount;
}
// Create a proxy in the tree as a leaf node. We return the index
// of the node instead of a pointer so that we can grow
// the node pool.
int32 b2DynamicTree::CreateProxy(const b2AABB& aabb, void* userData)
{
int32 proxyId = AllocateNode();
// Fatten the aabb.
b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
m_nodes[proxyId].aabb.lowerBound = aabb.lowerBound - r;
m_nodes[proxyId].aabb.upperBound = aabb.upperBound + r;
m_nodes[proxyId].userData = userData;
m_nodes[proxyId].height = 0;
InsertLeaf(proxyId);
return proxyId;
}
void b2DynamicTree::DestroyProxy(int32 proxyId)
{
b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
b2Assert(m_nodes[proxyId].IsLeaf());
RemoveLeaf(proxyId);
FreeNode(proxyId);
}
bool b2DynamicTree::MoveProxy(int32 proxyId, const b2AABB& aabb, const b2Vec2& displacement)
{
b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
b2Assert(m_nodes[proxyId].IsLeaf());
if (m_nodes[proxyId].aabb.Contains(aabb))
{
return false;
}
RemoveLeaf(proxyId);
// Extend AABB.
b2AABB b = aabb;
b2Vec2 r(b2_aabbExtension, b2_aabbExtension);
b.lowerBound = b.lowerBound - r;
b.upperBound = b.upperBound + r;
// Predict AABB displacement.
b2Vec2 d = b2_aabbMultiplier * displacement;
if (d.x < 0.0f)
{
b.lowerBound.x += d.x;
}
else
{
b.upperBound.x += d.x;
}
if (d.y < 0.0f)
{
b.lowerBound.y += d.y;
}
else
{
b.upperBound.y += d.y;
}
m_nodes[proxyId].aabb = b;
InsertLeaf(proxyId);
return true;
}
void b2DynamicTree::InsertLeaf(int32 leaf)
{
++m_insertionCount;
if (m_root == b2_nullNode)
{
m_root = leaf;
m_nodes[m_root].parent = b2_nullNode;
return;
}
// Find the best sibling for this node
b2AABB leafAABB = m_nodes[leaf].aabb;
int32 index = m_root;
while (m_nodes[index].IsLeaf() == false)
{
int32 child1 = m_nodes[index].child1;
int32 child2 = m_nodes[index].child2;
float32 area = m_nodes[index].aabb.GetPerimeter();
b2AABB combinedAABB;
combinedAABB.Combine(m_nodes[index].aabb, leafAABB);
float32 combinedArea = combinedAABB.GetPerimeter();
// Cost of creating a new parent for this node and the new leaf
float32 cost = 2.0f * combinedArea;
// Minimum cost of pushing the leaf further down the tree
float32 inheritanceCost = 2.0f * (combinedArea - area);
// Cost of descending into child1
float32 cost1;
if (m_nodes[child1].IsLeaf())
{
b2AABB aabb;
aabb.Combine(leafAABB, m_nodes[child1].aabb);
cost1 = aabb.GetPerimeter() + inheritanceCost;
}
else
{
b2AABB aabb;
aabb.Combine(leafAABB, m_nodes[child1].aabb);
float32 oldArea = m_nodes[child1].aabb.GetPerimeter();
float32 newArea = aabb.GetPerimeter();
cost1 = (newArea - oldArea) + inheritanceCost;
}
// Cost of descending into child2
float32 cost2;
if (m_nodes[child2].IsLeaf())
{
b2AABB aabb;
aabb.Combine(leafAABB, m_nodes[child2].aabb);
cost2 = aabb.GetPerimeter() + inheritanceCost;
}
else
{
b2AABB aabb;
aabb.Combine(leafAABB, m_nodes[child2].aabb);
float32 oldArea = m_nodes[child2].aabb.GetPerimeter();
float32 newArea = aabb.GetPerimeter();
cost2 = newArea - oldArea + inheritanceCost;
}
// Descend according to the minimum cost.
if (cost < cost1 && cost < cost2)
{
break;
}
// Descend
if (cost1 < cost2)
{
index = child1;
}
else
{
index = child2;
}
}
int32 sibling = index;
// Create a new parent.
int32 oldParent = m_nodes[sibling].parent;
int32 newParent = AllocateNode();
m_nodes[newParent].parent = oldParent;
m_nodes[newParent].userData = NULL;
m_nodes[newParent].aabb.Combine(leafAABB, m_nodes[sibling].aabb);
m_nodes[newParent].height = m_nodes[sibling].height + 1;
if (oldParent != b2_nullNode)
{
// The sibling was not the root.
if (m_nodes[oldParent].child1 == sibling)
{
m_nodes[oldParent].child1 = newParent;
}
else
{
m_nodes[oldParent].child2 = newParent;
}
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parent = newParent;
m_nodes[leaf].parent = newParent;
}
else
{
// The sibling was the root.
m_nodes[newParent].child1 = sibling;
m_nodes[newParent].child2 = leaf;
m_nodes[sibling].parent = newParent;
m_nodes[leaf].parent = newParent;
m_root = newParent;
}
// Walk back up the tree fixing heights and AABBs
index = m_nodes[leaf].parent;
while (index != b2_nullNode)
{
index = Balance(index);
int32 child1 = m_nodes[index].child1;
int32 child2 = m_nodes[index].child2;
b2Assert(child1 != b2_nullNode);
b2Assert(child2 != b2_nullNode);
m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
index = m_nodes[index].parent;
}
//Validate();
}
void b2DynamicTree::RemoveLeaf(int32 leaf)
{
if (leaf == m_root)
{
m_root = b2_nullNode;
return;
}
int32 parent = m_nodes[leaf].parent;
int32 grandParent = m_nodes[parent].parent;
int32 sibling;
if (m_nodes[parent].child1 == leaf)
{
sibling = m_nodes[parent].child2;
}
else
{
sibling = m_nodes[parent].child1;
}
if (grandParent != b2_nullNode)
{
// Destroy parent and connect sibling to grandParent.
if (m_nodes[grandParent].child1 == parent)
{
m_nodes[grandParent].child1 = sibling;
}
else
{
m_nodes[grandParent].child2 = sibling;
}
m_nodes[sibling].parent = grandParent;
FreeNode(parent);
// Adjust ancestor bounds.
int32 index = grandParent;
while (index != b2_nullNode)
{
index = Balance(index);
int32 child1 = m_nodes[index].child1;
int32 child2 = m_nodes[index].child2;
m_nodes[index].aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
m_nodes[index].height = 1 + b2Max(m_nodes[child1].height, m_nodes[child2].height);
index = m_nodes[index].parent;
}
}
else
{
m_root = sibling;
m_nodes[sibling].parent = b2_nullNode;
FreeNode(parent);
}
//Validate();
}
// Perform a left or right rotation if node A is imbalanced.
// Returns the new root index.
int32 b2DynamicTree::Balance(int32 iA)
{
b2Assert(iA != b2_nullNode);
b2TreeNode* A = m_nodes + iA;
if (A->IsLeaf() || A->height < 2)
{
return iA;
}
int32 iB = A->child1;
int32 iC = A->child2;
b2Assert(0 <= iB && iB < m_nodeCapacity);
b2Assert(0 <= iC && iC < m_nodeCapacity);
b2TreeNode* B = m_nodes + iB;
b2TreeNode* C = m_nodes + iC;
int32 balance = C->height - B->height;
// Rotate C up
if (balance > 1)
{
int32 iF = C->child1;
int32 iG = C->child2;
b2TreeNode* F = m_nodes + iF;
b2TreeNode* G = m_nodes + iG;
b2Assert(0 <= iF && iF < m_nodeCapacity);
b2Assert(0 <= iG && iG < m_nodeCapacity);
// Swap A and C
C->child1 = iA;
C->parent = A->parent;
A->parent = iC;
// A's old parent should point to C
if (C->parent != b2_nullNode)
{
if (m_nodes[C->parent].child1 == iA)
{
m_nodes[C->parent].child1 = iC;
}
else
{
b2Assert(m_nodes[C->parent].child2 == iA);
m_nodes[C->parent].child2 = iC;
}
}
else
{
m_root = iC;
}
// Rotate
if (F->height > G->height)
{
C->child2 = iF;
A->child2 = iG;
G->parent = iA;
A->aabb.Combine(B->aabb, G->aabb);
C->aabb.Combine(A->aabb, F->aabb);
A->height = 1 + b2Max(B->height, G->height);
C->height = 1 + b2Max(A->height, F->height);
}
else
{
C->child2 = iG;
A->child2 = iF;
F->parent = iA;
A->aabb.Combine(B->aabb, F->aabb);
C->aabb.Combine(A->aabb, G->aabb);
A->height = 1 + b2Max(B->height, F->height);
C->height = 1 + b2Max(A->height, G->height);
}
return iC;
}
// Rotate B up
if (balance < -1)
{
int32 iD = B->child1;
int32 iE = B->child2;
b2TreeNode* D = m_nodes + iD;
b2TreeNode* E = m_nodes + iE;
b2Assert(0 <= iD && iD < m_nodeCapacity);
b2Assert(0 <= iE && iE < m_nodeCapacity);
// Swap A and B
B->child1 = iA;
B->parent = A->parent;
A->parent = iB;
// A's old parent should point to B
if (B->parent != b2_nullNode)
{
if (m_nodes[B->parent].child1 == iA)
{
m_nodes[B->parent].child1 = iB;
}
else
{
b2Assert(m_nodes[B->parent].child2 == iA);
m_nodes[B->parent].child2 = iB;
}
}
else
{
m_root = iB;
}
// Rotate
if (D->height > E->height)
{
B->child2 = iD;
A->child1 = iE;
E->parent = iA;
A->aabb.Combine(C->aabb, E->aabb);
B->aabb.Combine(A->aabb, D->aabb);
A->height = 1 + b2Max(C->height, E->height);
B->height = 1 + b2Max(A->height, D->height);
}
else
{
B->child2 = iE;
A->child1 = iD;
D->parent = iA;
A->aabb.Combine(C->aabb, D->aabb);
B->aabb.Combine(A->aabb, E->aabb);
A->height = 1 + b2Max(C->height, D->height);
B->height = 1 + b2Max(A->height, E->height);
}
return iB;
}
return iA;
}
int32 b2DynamicTree::GetHeight() const
{
if (m_root == b2_nullNode)
{
return 0;
}
return m_nodes[m_root].height;
}
//
float32 b2DynamicTree::GetAreaRatio() const
{
if (m_root == b2_nullNode)
{
return 0.0f;
}
const b2TreeNode* root = m_nodes + m_root;
float32 rootArea = root->aabb.GetPerimeter();
float32 totalArea = 0.0f;
for (int32 i = 0; i < m_nodeCapacity; ++i)
{
const b2TreeNode* node = m_nodes + i;
if (node->height < 0)
{
// Free node in pool
continue;
}
totalArea += node->aabb.GetPerimeter();
}
return totalArea / rootArea;
}
// Compute the height of a sub-tree.
int32 b2DynamicTree::ComputeHeight(int32 nodeId) const
{
b2Assert(0 <= nodeId && nodeId < m_nodeCapacity);
b2TreeNode* node = m_nodes + nodeId;
if (node->IsLeaf())
{
return 0;
}
int32 height1 = ComputeHeight(node->child1);
int32 height2 = ComputeHeight(node->child2);
return 1 + b2Max(height1, height2);
}
int32 b2DynamicTree::ComputeHeight() const
{
int32 height = ComputeHeight(m_root);
return height;
}
void b2DynamicTree::ValidateStructure(int32 index) const
{
if (index == b2_nullNode)
{
return;
}
if (index == m_root)
{
b2Assert(m_nodes[index].parent == b2_nullNode);
}
const b2TreeNode* node = m_nodes + index;
int32 child1 = node->child1;
int32 child2 = node->child2;
if (node->IsLeaf())
{
b2Assert(child1 == b2_nullNode);
b2Assert(child2 == b2_nullNode);
b2Assert(node->height == 0);
return;
}
b2Assert(0 <= child1 && child1 < m_nodeCapacity);
b2Assert(0 <= child2 && child2 < m_nodeCapacity);
b2Assert(m_nodes[child1].parent == index);
b2Assert(m_nodes[child2].parent == index);
ValidateStructure(child1);
ValidateStructure(child2);
}
void b2DynamicTree::ValidateMetrics(int32 index) const
{
if (index == b2_nullNode)
{
return;
}
const b2TreeNode* node = m_nodes + index;
int32 child1 = node->child1;
int32 child2 = node->child2;
if (node->IsLeaf())
{
b2Assert(child1 == b2_nullNode);
b2Assert(child2 == b2_nullNode);
b2Assert(node->height == 0);
return;
}
b2Assert(0 <= child1 && child1 < m_nodeCapacity);
b2Assert(0 <= child2 && child2 < m_nodeCapacity);
int32 height1 = m_nodes[child1].height;
int32 height2 = m_nodes[child2].height;
int32 height;
height = 1 + b2Max(height1, height2);
b2Assert(node->height == height);
b2AABB aabb;
aabb.Combine(m_nodes[child1].aabb, m_nodes[child2].aabb);
b2Assert(aabb.lowerBound == node->aabb.lowerBound);
b2Assert(aabb.upperBound == node->aabb.upperBound);
ValidateMetrics(child1);
ValidateMetrics(child2);
}
void b2DynamicTree::Validate() const
{
ValidateStructure(m_root);
ValidateMetrics(m_root);
int32 freeCount = 0;
int32 freeIndex = m_freeList;
while (freeIndex != b2_nullNode)
{
b2Assert(0 <= freeIndex && freeIndex < m_nodeCapacity);
freeIndex = m_nodes[freeIndex].next;
++freeCount;
}
b2Assert(GetHeight() == ComputeHeight());
b2Assert(m_nodeCount + freeCount == m_nodeCapacity);
}
int32 b2DynamicTree::GetMaxBalance() const
{
int32 maxBalance = 0;
for (int32 i = 0; i < m_nodeCapacity; ++i)
{
const b2TreeNode* node = m_nodes + i;
if (node->height <= 1)
{
continue;
}
b2Assert(node->IsLeaf() == false);
int32 child1 = node->child1;
int32 child2 = node->child2;
int32 balance = b2Abs(m_nodes[child2].height - m_nodes[child1].height);
maxBalance = b2Max(maxBalance, balance);
}
return maxBalance;
}
void b2DynamicTree::RebuildBottomUp()
{
int32* nodes = (int32*)b2Alloc(m_nodeCount * sizeof(int32));
int32 count = 0;
// Build array of leaves. Free the rest.
for (int32 i = 0; i < m_nodeCapacity; ++i)
{
if (m_nodes[i].height < 0)
{
// free node in pool
continue;
}
if (m_nodes[i].IsLeaf())
{
m_nodes[i].parent = b2_nullNode;
nodes[count] = i;
++count;
}
else
{
FreeNode(i);
}
}
while (count > 1)
{
float32 minCost = b2_maxFloat;
int32 iMin = -1, jMin = -1;
for (int32 i = 0; i < count; ++i)
{
b2AABB aabbi = m_nodes[nodes[i]].aabb;
for (int32 j = i + 1; j < count; ++j)
{
b2AABB aabbj = m_nodes[nodes[j]].aabb;
b2AABB b;
b.Combine(aabbi, aabbj);
float32 cost = b.GetPerimeter();
if (cost < minCost)
{
iMin = i;
jMin = j;
minCost = cost;
}
}
}
int32 index1 = nodes[iMin];
int32 index2 = nodes[jMin];
b2TreeNode* child1 = m_nodes + index1;
b2TreeNode* child2 = m_nodes + index2;
int32 parentIndex = AllocateNode();
b2TreeNode* parent = m_nodes + parentIndex;
parent->child1 = index1;
parent->child2 = index2;
parent->height = 1 + b2Max(child1->height, child2->height);
parent->aabb.Combine(child1->aabb, child2->aabb);
parent->parent = b2_nullNode;
child1->parent = parentIndex;
child2->parent = parentIndex;
nodes[jMin] = nodes[count-1];
nodes[iMin] = parentIndex;
--count;
}
m_root = nodes[0];
b2Free(nodes);
Validate();
}
void b2DynamicTree::ShiftOrigin(const b2Vec2& newOrigin)
{
// Build array of leaves. Free the rest.
for (int32 i = 0; i < m_nodeCapacity; ++i)
{
m_nodes[i].aabb.lowerBound -= newOrigin;
m_nodes[i].aabb.upperBound -= newOrigin;
}
}
+289
View File
@@ -0,0 +1,289 @@
/*
* Copyright (c) 2009 Erin Catto http://www.box2d.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 B2_DYNAMIC_TREE_H
#define B2_DYNAMIC_TREE_H
#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Common/b2GrowableStack.h>
#define b2_nullNode (-1)
/// A node in the dynamic tree. The client does not interact with this directly.
struct b2TreeNode
{
bool IsLeaf() const
{
return child1 == b2_nullNode;
}
/// Enlarged AABB
b2AABB aabb;
void* userData;
union
{
int32 parent;
int32 next;
};
int32 child1;
int32 child2;
// leaf = 0, free node = -1
int32 height;
};
/// A dynamic AABB tree broad-phase, inspired by Nathanael Presson's btDbvt.
/// A dynamic tree arranges data in a binary tree to accelerate
/// queries such as volume queries and ray casts. Leafs are proxies
/// with an AABB. In the tree we expand the proxy AABB by b2_fatAABBFactor
/// so that the proxy AABB is bigger than the client object. This allows the client
/// object to move by small amounts without triggering a tree update.
///
/// Nodes are pooled and relocatable, so we use node indices rather than pointers.
class b2DynamicTree
{
public:
/// Constructing the tree initializes the node pool.
b2DynamicTree();
/// Destroy the tree, freeing the node pool.
~b2DynamicTree();
/// Create a proxy. Provide a tight fitting AABB and a userData pointer.
int32 CreateProxy(const b2AABB& aabb, void* userData);
/// Destroy a proxy. This asserts if the id is invalid.
void DestroyProxy(int32 proxyId);
/// Move a proxy with a swepted AABB. If the proxy has moved outside of its fattened AABB,
/// then the proxy is removed from the tree and re-inserted. Otherwise
/// the function returns immediately.
/// @return true if the proxy was re-inserted.
bool MoveProxy(int32 proxyId, const b2AABB& aabb1, const b2Vec2& displacement);
/// Get proxy user data.
/// @return the proxy user data or 0 if the id is invalid.
void* GetUserData(int32 proxyId) const;
/// Get the fat AABB for a proxy.
const b2AABB& GetFatAABB(int32 proxyId) const;
/// Query an AABB for overlapping proxies. The callback class
/// is called for each proxy that overlaps the supplied AABB.
template <typename T>
void Query(T* callback, const b2AABB& aabb) const;
/// Ray-cast against the proxies in the tree. This relies on the callback
/// to perform a exact ray-cast in the case were the proxy contains a shape.
/// The callback also performs the any collision filtering. This has performance
/// roughly equal to k * log(n), where k is the number of collisions and n is the
/// number of proxies in the tree.
/// @param input the ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).
/// @param callback a callback class that is called for each proxy that is hit by the ray.
template <typename T>
void RayCast(T* callback, const b2RayCastInput& input) const;
/// Validate this tree. For testing.
void Validate() const;
/// Compute the height of the binary tree in O(N) time. Should not be
/// called often.
int32 GetHeight() const;
/// Get the maximum balance of an node in the tree. The balance is the difference
/// in height of the two children of a node.
int32 GetMaxBalance() const;
/// Get the ratio of the sum of the node areas to the root area.
float32 GetAreaRatio() const;
/// Build an optimal tree. Very expensive. For testing.
void RebuildBottomUp();
/// Shift the world origin. Useful for large worlds.
/// The shift formula is: position -= newOrigin
/// @param newOrigin the new origin with respect to the old origin
void ShiftOrigin(const b2Vec2& newOrigin);
private:
int32 AllocateNode();
void FreeNode(int32 node);
void InsertLeaf(int32 node);
void RemoveLeaf(int32 node);
int32 Balance(int32 index);
int32 ComputeHeight() const;
int32 ComputeHeight(int32 nodeId) const;
void ValidateStructure(int32 index) const;
void ValidateMetrics(int32 index) const;
int32 m_root;
b2TreeNode* m_nodes;
int32 m_nodeCount;
int32 m_nodeCapacity;
int32 m_freeList;
/// This is used to incrementally traverse the tree for re-balancing.
uint32 m_path;
int32 m_insertionCount;
};
inline void* b2DynamicTree::GetUserData(int32 proxyId) const
{
b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
return m_nodes[proxyId].userData;
}
inline const b2AABB& b2DynamicTree::GetFatAABB(int32 proxyId) const
{
b2Assert(0 <= proxyId && proxyId < m_nodeCapacity);
return m_nodes[proxyId].aabb;
}
template <typename T>
inline void b2DynamicTree::Query(T* callback, const b2AABB& aabb) const
{
b2GrowableStack<int32, 256> stack;
stack.Push(m_root);
while (stack.GetCount() > 0)
{
int32 nodeId = stack.Pop();
if (nodeId == b2_nullNode)
{
continue;
}
const b2TreeNode* node = m_nodes + nodeId;
if (b2TestOverlap(node->aabb, aabb))
{
if (node->IsLeaf())
{
bool proceed = callback->QueryCallback(nodeId);
if (proceed == false)
{
return;
}
}
else
{
stack.Push(node->child1);
stack.Push(node->child2);
}
}
}
}
template <typename T>
inline void b2DynamicTree::RayCast(T* callback, const b2RayCastInput& input) const
{
b2Vec2 p1 = input.p1;
b2Vec2 p2 = input.p2;
b2Vec2 r = p2 - p1;
b2Assert(r.LengthSquared() > 0.0f);
r.Normalize();
// v is perpendicular to the segment.
b2Vec2 v = b2Cross(1.0f, r);
b2Vec2 abs_v = b2Abs(v);
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
float32 maxFraction = input.maxFraction;
// Build a bounding box for the segment.
b2AABB segmentAABB;
{
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = b2Min(p1, t);
segmentAABB.upperBound = b2Max(p1, t);
}
b2GrowableStack<int32, 256> stack;
stack.Push(m_root);
while (stack.GetCount() > 0)
{
int32 nodeId = stack.Pop();
if (nodeId == b2_nullNode)
{
continue;
}
const b2TreeNode* node = m_nodes + nodeId;
if (b2TestOverlap(node->aabb, segmentAABB) == false)
{
continue;
}
// Separating axis for segment (Gino, p80).
// |dot(v, p1 - c)| > dot(|v|, h)
b2Vec2 c = node->aabb.GetCenter();
b2Vec2 h = node->aabb.GetExtents();
float32 separation = b2Abs(b2Dot(v, p1 - c)) - b2Dot(abs_v, h);
if (separation > 0.0f)
{
continue;
}
if (node->IsLeaf())
{
b2RayCastInput subInput;
subInput.p1 = input.p1;
subInput.p2 = input.p2;
subInput.maxFraction = maxFraction;
float32 value = callback->RayCastCallback(subInput, nodeId);
if (value == 0.0f)
{
// The client has terminated the ray cast.
return;
}
if (value > 0.0f)
{
// Update segment bounding box.
maxFraction = value;
b2Vec2 t = p1 + maxFraction * (p2 - p1);
segmentAABB.lowerBound = b2Min(p1, t);
segmentAABB.upperBound = b2Max(p1, t);
}
}
else
{
stack.Push(node->child1);
stack.Push(node->child2);
}
}
}
#endif
+486
View File
@@ -0,0 +1,486 @@
/*
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.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 <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/b2Distance.h>
#include <Box2D/Collision/b2TimeOfImpact.h>
#include <Box2D/Collision/Shapes/b2CircleShape.h>
#include <Box2D/Collision/Shapes/b2PolygonShape.h>
#include <Box2D/Common/b2Timer.h>
#include <stdio.h>
float32 b2_toiTime, b2_toiMaxTime;
int32 b2_toiCalls, b2_toiIters, b2_toiMaxIters;
int32 b2_toiRootIters, b2_toiMaxRootIters;
//
struct b2SeparationFunction
{
enum Type
{
e_points,
e_faceA,
e_faceB
};
// TODO_ERIN might not need to return the separation
float32 Initialize(const b2SimplexCache* cache,
const b2DistanceProxy* proxyA, const b2Sweep& sweepA,
const b2DistanceProxy* proxyB, const b2Sweep& sweepB,
float32 t1)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int32 count = cache->count;
b2Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t1);
m_sweepB.GetTransform(&xfB, t1);
if (count == 1)
{
m_type = e_points;
b2Vec2 localPointA = m_proxyA->GetVertex(cache->indexA[0]);
b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
m_axis = pointB - pointA;
float32 s = m_axis.Normalize();
return s;
}
else if (cache->indexA[0] == cache->indexA[1])
{
// Two points on B and one on A.
m_type = e_faceB;
b2Vec2 localPointB1 = proxyB->GetVertex(cache->indexB[0]);
b2Vec2 localPointB2 = proxyB->GetVertex(cache->indexB[1]);
m_axis = b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Mul(xfB.q, m_axis);
m_localPoint = 0.5f * (localPointB1 + localPointB2);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
b2Vec2 localPointA = proxyA->GetVertex(cache->indexA[0]);
b2Vec2 pointA = b2Mul(xfA, localPointA);
float32 s = b2Dot(pointA - pointB, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
else
{
// Two points on A and one or two points on B.
m_type = e_faceA;
b2Vec2 localPointA1 = m_proxyA->GetVertex(cache->indexA[0]);
b2Vec2 localPointA2 = m_proxyA->GetVertex(cache->indexA[1]);
m_axis = b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.Normalize();
b2Vec2 normal = b2Mul(xfA.q, m_axis);
m_localPoint = 0.5f * (localPointA1 + localPointA2);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
b2Vec2 localPointB = m_proxyB->GetVertex(cache->indexB[0]);
b2Vec2 pointB = b2Mul(xfB, localPointB);
float32 s = b2Dot(pointB - pointA, normal);
if (s < 0.0f)
{
m_axis = -m_axis;
s = -s;
}
return s;
}
}
//
float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
{
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t);
m_sweepB.GetTransform(&xfB, t);
switch (m_type)
{
case e_points:
{
b2Vec2 axisA = b2MulT(xfA.q, m_axis);
b2Vec2 axisB = b2MulT(xfB.q, -m_axis);
*indexA = m_proxyA->GetSupport(axisA);
*indexB = m_proxyB->GetSupport(axisB);
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
float32 separation = b2Dot(pointB - pointA, m_axis);
return separation;
}
case e_faceA:
{
b2Vec2 normal = b2Mul(xfA.q, m_axis);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
b2Vec2 axisB = b2MulT(xfB.q, -normal);
*indexA = -1;
*indexB = m_proxyB->GetSupport(axisB);
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
b2Vec2 pointB = b2Mul(xfB, localPointB);
float32 separation = b2Dot(pointB - pointA, normal);
return separation;
}
case e_faceB:
{
b2Vec2 normal = b2Mul(xfB.q, m_axis);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
b2Vec2 axisA = b2MulT(xfA.q, -normal);
*indexB = -1;
*indexA = m_proxyA->GetSupport(axisA);
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
b2Vec2 pointA = b2Mul(xfA, localPointA);
float32 separation = b2Dot(pointA - pointB, normal);
return separation;
}
default:
b2Assert(false);
*indexA = -1;
*indexB = -1;
return 0.0f;
}
}
//
float32 Evaluate(int32 indexA, int32 indexB, float32 t) const
{
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t);
m_sweepB.GetTransform(&xfB, t);
switch (m_type)
{
case e_points:
{
b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
float32 separation = b2Dot(pointB - pointA, m_axis);
return separation;
}
case e_faceA:
{
b2Vec2 normal = b2Mul(xfA.q, m_axis);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
b2Vec2 localPointB = m_proxyB->GetVertex(indexB);
b2Vec2 pointB = b2Mul(xfB, localPointB);
float32 separation = b2Dot(pointB - pointA, normal);
return separation;
}
case e_faceB:
{
b2Vec2 normal = b2Mul(xfB.q, m_axis);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
b2Vec2 localPointA = m_proxyA->GetVertex(indexA);
b2Vec2 pointA = b2Mul(xfA, localPointA);
float32 separation = b2Dot(pointA - pointB, normal);
return separation;
}
default:
b2Assert(false);
return 0.0f;
}
}
const b2DistanceProxy* m_proxyA;
const b2DistanceProxy* m_proxyB;
b2Sweep m_sweepA, m_sweepB;
Type m_type;
b2Vec2 m_localPoint;
b2Vec2 m_axis;
};
// CCD via the local separating axis method. This seeks progression
// by computing the largest time at which separation is maintained.
void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input)
{
b2Timer timer;
++b2_toiCalls;
output->state = b2TOIOutput::e_unknown;
output->t = input->tMax;
const b2DistanceProxy* proxyA = &input->proxyA;
const b2DistanceProxy* proxyB = &input->proxyB;
b2Sweep sweepA = input->sweepA;
b2Sweep sweepB = input->sweepB;
// Large rotations can make the root finder fail, so we normalize the
// sweep angles.
sweepA.Normalize();
sweepB.Normalize();
float32 tMax = input->tMax;
float32 totalRadius = proxyA->m_radius + proxyB->m_radius;
float32 target = b2Max(b2_linearSlop, totalRadius - 3.0f * b2_linearSlop);
float32 tolerance = 0.25f * b2_linearSlop;
b2Assert(target > tolerance);
float32 t1 = 0.0f;
const int32 k_maxIterations = 20; // TODO_ERIN b2Settings
int32 iter = 0;
// Prepare input for distance query.
b2SimplexCache cache;
cache.count = 0;
b2DistanceInput distanceInput;
distanceInput.proxyA = input->proxyA;
distanceInput.proxyB = input->proxyB;
distanceInput.useRadii = false;
// The outer loop progressively attempts to compute new separating axes.
// This loop terminates when an axis is repeated (no progress is made).
for(;;)
{
b2Transform xfA, xfB;
sweepA.GetTransform(&xfA, t1);
sweepB.GetTransform(&xfB, t1);
// Get the distance between shapes. We can also use the results
// to get a separating axis.
distanceInput.transformA = xfA;
distanceInput.transformB = xfB;
b2DistanceOutput distanceOutput;
b2Distance(&distanceOutput, &cache, &distanceInput);
// If the shapes are overlapped, we give up on continuous collision.
if (distanceOutput.distance <= 0.0f)
{
// Failure!
output->state = b2TOIOutput::e_overlapped;
output->t = 0.0f;
break;
}
if (distanceOutput.distance < target + tolerance)
{
// Victory!
output->state = b2TOIOutput::e_touching;
output->t = t1;
break;
}
// Initialize the separating axis.
b2SeparationFunction fcn;
fcn.Initialize(&cache, proxyA, sweepA, proxyB, sweepB, t1);
#if 0
// Dump the curve seen by the root finder
{
const int32 N = 100;
float32 dx = 1.0f / N;
float32 xs[N+1];
float32 fs[N+1];
float32 x = 0.0f;
for (int32 i = 0; i <= N; ++i)
{
sweepA.GetTransform(&xfA, x);
sweepB.GetTransform(&xfB, x);
float32 f = fcn.Evaluate(xfA, xfB) - target;
printf("%g %g\n", x, f);
xs[i] = x;
fs[i] = f;
x += dx;
}
}
#endif
// Compute the TOI on the separating axis. We do this by successively
// resolving the deepest point. This loop is bounded by the number of vertices.
bool done = false;
float32 t2 = tMax;
int32 pushBackIter = 0;
for (;;)
{
// Find the deepest point at t2. Store the witness point indices.
int32 indexA, indexB;
float32 s2 = fcn.FindMinSeparation(&indexA, &indexB, t2);
// Is the final configuration separated?
if (s2 > target + tolerance)
{
// Victory!
output->state = b2TOIOutput::e_separated;
output->t = tMax;
done = true;
break;
}
// Has the separation reached tolerance?
if (s2 > target - tolerance)
{
// Advance the sweeps
t1 = t2;
break;
}
// Compute the initial separation of the witness points.
float32 s1 = fcn.Evaluate(indexA, indexB, t1);
// Check for initial overlap. This might happen if the root finder
// runs out of iterations.
if (s1 < target - tolerance)
{
output->state = b2TOIOutput::e_failed;
output->t = t1;
done = true;
break;
}
// Check for touching
if (s1 <= target + tolerance)
{
// Victory! t1 should hold the TOI (could be 0.0).
output->state = b2TOIOutput::e_touching;
output->t = t1;
done = true;
break;
}
// Compute 1D root of: f(x) - target = 0
int32 rootIterCount = 0;
float32 a1 = t1, a2 = t2;
for (;;)
{
// Use a mix of the secant rule and bisection.
float32 t;
if (rootIterCount & 1)
{
// Secant rule to improve convergence.
t = a1 + (target - s1) * (a2 - a1) / (s2 - s1);
}
else
{
// Bisection to guarantee progress.
t = 0.5f * (a1 + a2);
}
++rootIterCount;
++b2_toiRootIters;
float32 s = fcn.Evaluate(indexA, indexB, t);
if (b2Abs(s - target) < tolerance)
{
// t2 holds a tentative value for t1
t2 = t;
break;
}
// Ensure we continue to bracket the root.
if (s > target)
{
a1 = t;
s1 = s;
}
else
{
a2 = t;
s2 = s;
}
if (rootIterCount == 50)
{
break;
}
}
b2_toiMaxRootIters = b2Max(b2_toiMaxRootIters, rootIterCount);
++pushBackIter;
if (pushBackIter == b2_maxPolygonVertices)
{
break;
}
}
++iter;
++b2_toiIters;
if (done)
{
break;
}
if (iter == k_maxIterations)
{
// Root finder got stuck. Semi-victory.
output->state = b2TOIOutput::e_failed;
output->t = t1;
break;
}
}
b2_toiMaxIters = b2Max(b2_toiMaxIters, iter);
float32 time = timer.GetMilliseconds();
b2_toiMaxTime = b2Max(b2_toiMaxTime, time);
b2_toiTime += time;
}
+58
View File
@@ -0,0 +1,58 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_TIME_OF_IMPACT_H
#define B2_TIME_OF_IMPACT_H
#include <Box2D/Common/b2Math.h>
#include <Box2D/Collision/b2Distance.h>
/// Input parameters for b2TimeOfImpact
struct b2TOIInput
{
b2DistanceProxy proxyA;
b2DistanceProxy proxyB;
b2Sweep sweepA;
b2Sweep sweepB;
float32 tMax; // defines sweep interval [0, tMax]
};
// Output parameters for b2TimeOfImpact.
struct b2TOIOutput
{
enum State
{
e_unknown,
e_failed,
e_overlapped,
e_touching,
e_separated
};
State state;
float32 t;
};
/// Compute the upper bound on time before two shapes penetrate. Time is represented as
/// a fraction between [0,tMax]. This uses a swept separating axis and may miss some intermediate,
/// non-tunneling collision. If you change the time interval, you should call this function
/// again.
/// Note: use b2Distance to compute the contact point and normal at the time of impact.
void b2TimeOfImpact(b2TOIOutput* output, const b2TOIInput* input);
#endif
+215
View File
@@ -0,0 +1,215 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Common/b2BlockAllocator.h>
#include <limits.h>
#include <memory.h>
#include <stddef.h>
int32 b2BlockAllocator::s_blockSizes[b2_blockSizes] =
{
16, // 0
32, // 1
64, // 2
96, // 3
128, // 4
160, // 5
192, // 6
224, // 7
256, // 8
320, // 9
384, // 10
448, // 11
512, // 12
640, // 13
};
uint8 b2BlockAllocator::s_blockSizeLookup[b2_maxBlockSize + 1];
bool b2BlockAllocator::s_blockSizeLookupInitialized;
struct b2Chunk
{
int32 blockSize;
b2Block* blocks;
};
struct b2Block
{
b2Block* next;
};
b2BlockAllocator::b2BlockAllocator()
{
b2Assert(b2_blockSizes < UCHAR_MAX);
m_chunkSpace = b2_chunkArrayIncrement;
m_chunkCount = 0;
m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
memset(m_freeLists, 0, sizeof(m_freeLists));
if (s_blockSizeLookupInitialized == false)
{
int32 j = 0;
for (int32 i = 1; i <= b2_maxBlockSize; ++i)
{
b2Assert(j < b2_blockSizes);
if (i <= s_blockSizes[j])
{
s_blockSizeLookup[i] = (uint8)j;
}
else
{
++j;
s_blockSizeLookup[i] = (uint8)j;
}
}
s_blockSizeLookupInitialized = true;
}
}
b2BlockAllocator::~b2BlockAllocator()
{
for (int32 i = 0; i < m_chunkCount; ++i)
{
b2Free(m_chunks[i].blocks);
}
b2Free(m_chunks);
}
void* b2BlockAllocator::Allocate(int32 size)
{
if (size == 0)
return NULL;
b2Assert(0 < size);
if (size > b2_maxBlockSize)
{
return b2Alloc(size);
}
int32 index = s_blockSizeLookup[size];
b2Assert(0 <= index && index < b2_blockSizes);
if (m_freeLists[index])
{
b2Block* block = m_freeLists[index];
m_freeLists[index] = block->next;
return block;
}
else
{
if (m_chunkCount == m_chunkSpace)
{
b2Chunk* oldChunks = m_chunks;
m_chunkSpace += b2_chunkArrayIncrement;
m_chunks = (b2Chunk*)b2Alloc(m_chunkSpace * sizeof(b2Chunk));
memcpy(m_chunks, oldChunks, m_chunkCount * sizeof(b2Chunk));
memset(m_chunks + m_chunkCount, 0, b2_chunkArrayIncrement * sizeof(b2Chunk));
b2Free(oldChunks);
}
b2Chunk* chunk = m_chunks + m_chunkCount;
chunk->blocks = (b2Block*)b2Alloc(b2_chunkSize);
#if defined(_DEBUG)
memset(chunk->blocks, 0xcd, b2_chunkSize);
#endif
int32 blockSize = s_blockSizes[index];
chunk->blockSize = blockSize;
int32 blockCount = b2_chunkSize / blockSize;
b2Assert(blockCount * blockSize <= b2_chunkSize);
for (int32 i = 0; i < blockCount - 1; ++i)
{
b2Block* block = (b2Block*)((int8*)chunk->blocks + blockSize * i);
b2Block* next = (b2Block*)((int8*)chunk->blocks + blockSize * (i + 1));
block->next = next;
}
b2Block* last = (b2Block*)((int8*)chunk->blocks + blockSize * (blockCount - 1));
last->next = NULL;
m_freeLists[index] = chunk->blocks->next;
++m_chunkCount;
return chunk->blocks;
}
}
void b2BlockAllocator::Free(void* p, int32 size)
{
if (size == 0)
{
return;
}
b2Assert(0 < size);
if (size > b2_maxBlockSize)
{
b2Free(p);
return;
}
int32 index = s_blockSizeLookup[size];
b2Assert(0 <= index && index < b2_blockSizes);
#ifdef _DEBUG
// Verify the memory address and size is valid.
int32 blockSize = s_blockSizes[index];
bool found = false;
for (int32 i = 0; i < m_chunkCount; ++i)
{
b2Chunk* chunk = m_chunks + i;
if (chunk->blockSize != blockSize)
{
b2Assert( (int8*)p + blockSize <= (int8*)chunk->blocks ||
(int8*)chunk->blocks + b2_chunkSize <= (int8*)p);
}
else
{
if ((int8*)chunk->blocks <= (int8*)p && (int8*)p + blockSize <= (int8*)chunk->blocks + b2_chunkSize)
{
found = true;
}
}
}
b2Assert(found);
memset(p, 0xfd, blockSize);
#endif
b2Block* block = (b2Block*)p;
block->next = m_freeLists[index];
m_freeLists[index] = block;
}
void b2BlockAllocator::Clear()
{
for (int32 i = 0; i < m_chunkCount; ++i)
{
b2Free(m_chunks[i].blocks);
}
m_chunkCount = 0;
memset(m_chunks, 0, m_chunkSpace * sizeof(b2Chunk));
memset(m_freeLists, 0, sizeof(m_freeLists));
}
+62
View File
@@ -0,0 +1,62 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_BLOCK_ALLOCATOR_H
#define B2_BLOCK_ALLOCATOR_H
#include <Box2D/Common/b2Settings.h>
const int32 b2_chunkSize = 16 * 1024;
const int32 b2_maxBlockSize = 640;
const int32 b2_blockSizes = 14;
const int32 b2_chunkArrayIncrement = 128;
struct b2Block;
struct b2Chunk;
/// This is a small object allocator used for allocating small
/// objects that persist for more than one time step.
/// See: http://www.codeproject.com/useritems/Small_Block_Allocator.asp
class b2BlockAllocator
{
public:
b2BlockAllocator();
~b2BlockAllocator();
/// Allocate memory. This will use b2Alloc if the size is larger than b2_maxBlockSize.
void* Allocate(int32 size);
/// Free memory. This will use b2Free if the size is larger than b2_maxBlockSize.
void Free(void* p, int32 size);
void Clear();
private:
b2Chunk* m_chunks;
int32 m_chunkCount;
int32 m_chunkSpace;
b2Block* m_freeLists[b2_blockSizes];
static int32 s_blockSizes[b2_blockSizes];
static uint8 s_blockSizeLookup[b2_maxBlockSize + 1];
static bool s_blockSizeLookupInitialized;
};
#endif
+44
View File
@@ -0,0 +1,44 @@
/*
* Copyright (c) 2011 Erin Catto http://box2d.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 <Box2D/Common/b2Draw.h>
b2Draw::b2Draw()
{
m_drawFlags = 0;
}
void b2Draw::SetFlags(uint32 flags)
{
m_drawFlags = flags;
}
uint32 b2Draw::GetFlags() const
{
return m_drawFlags;
}
void b2Draw::AppendFlags(uint32 flags)
{
m_drawFlags |= flags;
}
void b2Draw::ClearFlags(uint32 flags)
{
m_drawFlags &= ~flags;
}
+86
View File
@@ -0,0 +1,86 @@
/*
* Copyright (c) 2011 Erin Catto http://box2d.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 B2_DRAW_H
#define B2_DRAW_H
#include <Box2D/Common/b2Math.h>
/// Color for debug drawing. Each value has the range [0,1].
struct b2Color
{
b2Color() {}
b2Color(float32 r, float32 g, float32 b) : r(r), g(g), b(b) {}
void Set(float32 ri, float32 gi, float32 bi) { r = ri; g = gi; b = bi; }
float32 r, g, b;
};
/// Implement and register this class with a b2World to provide debug drawing of physics
/// entities in your game.
class b2Draw
{
public:
b2Draw();
virtual ~b2Draw() {}
enum
{
e_shapeBit = 0x0001, ///< draw shapes
e_jointBit = 0x0002, ///< draw joint connections
e_aabbBit = 0x0004, ///< draw axis aligned bounding boxes
e_pairBit = 0x0008, ///< draw broad-phase pairs
e_centerOfMassBit = 0x0010 ///< draw center of mass frame
};
/// Set the drawing flags.
void SetFlags(uint32 flags);
/// Get the drawing flags.
uint32 GetFlags() const;
/// Append flags to the current flags.
void AppendFlags(uint32 flags);
/// Clear flags from the current flags.
void ClearFlags(uint32 flags);
/// Draw a closed polygon provided in CCW order.
virtual void DrawPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
/// Draw a solid closed polygon provided in CCW order.
virtual void DrawSolidPolygon(const b2Vec2* vertices, int32 vertexCount, const b2Color& color) = 0;
/// Draw a circle.
virtual void DrawCircle(const b2Vec2& center, float32 radius, const b2Color& color) = 0;
/// Draw a solid circle.
virtual void DrawSolidCircle(const b2Vec2& center, float32 radius, const b2Vec2& axis, const b2Color& color) = 0;
/// Draw a line segment.
virtual void DrawSegment(const b2Vec2& p1, const b2Vec2& p2, const b2Color& color) = 0;
/// Draw a transform. Choose your own length scale.
/// @param xf a transform.
virtual void DrawTransform(const b2Transform& xf) = 0;
protected:
uint32 m_drawFlags;
};
#endif
+85
View File
@@ -0,0 +1,85 @@
/*
* Copyright (c) 2010 Erin Catto http://www.box2d.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 B2_GROWABLE_STACK_H
#define B2_GROWABLE_STACK_H
#include <Box2D/Common/b2Settings.h>
#include <memory.h>
/// This is a growable LIFO stack with an initial capacity of N.
/// If the stack size exceeds the initial capacity, the heap is used
/// to increase the size of the stack.
template <typename T, int32 N>
class b2GrowableStack
{
public:
b2GrowableStack()
{
m_stack = m_array;
m_count = 0;
m_capacity = N;
}
~b2GrowableStack()
{
if (m_stack != m_array)
{
b2Free(m_stack);
m_stack = NULL;
}
}
void Push(const T& element)
{
if (m_count == m_capacity)
{
T* old = m_stack;
m_capacity *= 2;
m_stack = (T*)b2Alloc(m_capacity * sizeof(T));
memcpy(m_stack, old, m_count * sizeof(T));
if (old != m_array)
{
b2Free(old);
}
}
m_stack[m_count] = element;
++m_count;
}
T Pop()
{
b2Assert(m_count > 0);
--m_count;
return m_stack[m_count];
}
int32 GetCount()
{
return m_count;
}
private:
T* m_stack;
T m_array[N];
int32 m_count;
int32 m_capacity;
};
#endif
+94
View File
@@ -0,0 +1,94 @@
/*
* Copyright (c) 2007-2009 Erin Catto http://www.box2d.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 <Box2D/Common/b2Math.h>
const b2Vec2 b2Vec2_zero(0.0f, 0.0f);
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
b2Vec3 b2Mat33::Solve33(const b2Vec3& b) const
{
float32 det = b2Dot(ex, b2Cross(ey, ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
b2Vec3 x;
x.x = det * b2Dot(b, b2Cross(ey, ez));
x.y = det * b2Dot(ex, b2Cross(b, ez));
x.z = det * b2Dot(ex, b2Cross(ey, b));
return x;
}
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
b2Vec2 b2Mat33::Solve22(const b2Vec2& b) const
{
float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
float32 det = a11 * a22 - a12 * a21;
if (det != 0.0f)
{
det = 1.0f / det;
}
b2Vec2 x;
x.x = det * (a22 * b.x - a12 * b.y);
x.y = det * (a11 * b.y - a21 * b.x);
return x;
}
///
void b2Mat33::GetInverse22(b2Mat33* M) const
{
float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
float32 det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
M->ex.x = det * d; M->ey.x = -det * b; M->ex.z = 0.0f;
M->ex.y = -det * c; M->ey.y = det * a; M->ey.z = 0.0f;
M->ez.x = 0.0f; M->ez.y = 0.0f; M->ez.z = 0.0f;
}
/// Returns the zero matrix if singular.
void b2Mat33::GetSymInverse33(b2Mat33* M) const
{
float32 det = b2Dot(ex, b2Cross(ey, ez));
if (det != 0.0f)
{
det = 1.0f / det;
}
float32 a11 = ex.x, a12 = ey.x, a13 = ez.x;
float32 a22 = ey.y, a23 = ez.y;
float32 a33 = ez.z;
M->ex.x = det * (a22 * a33 - a23 * a23);
M->ex.y = det * (a13 * a23 - a12 * a33);
M->ex.z = det * (a12 * a23 - a13 * a22);
M->ey.x = M->ex.y;
M->ey.y = det * (a11 * a33 - a13 * a13);
M->ey.z = det * (a13 * a12 - a11 * a23);
M->ez.x = M->ex.z;
M->ez.y = M->ey.z;
M->ez.z = det * (a11 * a22 - a12 * a12);
}
+721
View File
@@ -0,0 +1,721 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_MATH_H
#define B2_MATH_H
#include <Box2D/Common/b2Settings.h>
#include <math.h>
#include <float.h>
/// This function is used to ensure that a floating point number is not a NaN or infinity.
inline bool b2IsValid(float32 x)
{
int32 ix = *reinterpret_cast<int32*>(&x);
return (ix & 0x7f800000) != 0x7f800000;
}
/// This is a approximate yet fast inverse square-root.
inline float32 b2InvSqrt(float32 x)
{
union
{
float32 x;
int32 i;
} convert;
convert.x = x;
float32 xhalf = 0.5f * x;
convert.i = 0x5f3759df - (convert.i >> 1);
x = convert.x;
x = x * (1.5f - xhalf * x * x);
return x;
}
#define b2Sqrt(x) sqrtf(x)
#define b2Atan2(y, x) atan2f(y, x)
/// A 2D column vector.
struct b2Vec2
{
/// Default constructor does nothing (for performance).
b2Vec2() {}
/// Construct using coordinates.
b2Vec2(float32 x, float32 y) : x(x), y(y) {}
/// Set this vector to all zeros.
void SetZero() { x = 0.0f; y = 0.0f; }
/// Set this vector to some specified coordinates.
void Set(float32 x_, float32 y_) { x = x_; y = y_; }
/// Negate this vector.
b2Vec2 operator -() const { b2Vec2 v; v.Set(-x, -y); return v; }
/// Read from and indexed element.
float32 operator () (int32 i) const
{
return (&x)[i];
}
/// Write to an indexed element.
float32& operator () (int32 i)
{
return (&x)[i];
}
/// Add a vector to this vector.
void operator += (const b2Vec2& v)
{
x += v.x; y += v.y;
}
/// Subtract a vector from this vector.
void operator -= (const b2Vec2& v)
{
x -= v.x; y -= v.y;
}
/// Multiply this vector by a scalar.
void operator *= (float32 a)
{
x *= a; y *= a;
}
/// Get the length of this vector (the norm).
float32 Length() const
{
return b2Sqrt(x * x + y * y);
}
/// Get the length squared. For performance, use this instead of
/// b2Vec2::Length (if possible).
float32 LengthSquared() const
{
return x * x + y * y;
}
/// Convert this vector into a unit vector. Returns the length.
float32 Normalize()
{
float32 length = Length();
if (length < b2_epsilon)
{
return 0.0f;
}
float32 invLength = 1.0f / length;
x *= invLength;
y *= invLength;
return length;
}
/// Does this vector contain finite coordinates?
bool IsValid() const
{
return b2IsValid(x) && b2IsValid(y);
}
/// Get the skew vector such that dot(skew_vec, other) == cross(vec, other)
b2Vec2 Skew() const
{
return b2Vec2(-y, x);
}
float32 x, y;
};
/// A 2D column vector with 3 elements.
struct b2Vec3
{
/// Default constructor does nothing (for performance).
b2Vec3() {}
/// Construct using coordinates.
b2Vec3(float32 x, float32 y, float32 z) : x(x), y(y), z(z) {}
/// Set this vector to all zeros.
void SetZero() { x = 0.0f; y = 0.0f; z = 0.0f; }
/// Set this vector to some specified coordinates.
void Set(float32 x_, float32 y_, float32 z_) { x = x_; y = y_; z = z_; }
/// Negate this vector.
b2Vec3 operator -() const { b2Vec3 v; v.Set(-x, -y, -z); return v; }
/// Add a vector to this vector.
void operator += (const b2Vec3& v)
{
x += v.x; y += v.y; z += v.z;
}
/// Subtract a vector from this vector.
void operator -= (const b2Vec3& v)
{
x -= v.x; y -= v.y; z -= v.z;
}
/// Multiply this vector by a scalar.
void operator *= (float32 s)
{
x *= s; y *= s; z *= s;
}
float32 x, y, z;
};
/// A 2-by-2 matrix. Stored in column-major order.
struct b2Mat22
{
/// The default constructor does nothing (for performance).
b2Mat22() {}
/// Construct this matrix using columns.
b2Mat22(const b2Vec2& c1, const b2Vec2& c2)
{
ex = c1;
ey = c2;
}
/// Construct this matrix using scalars.
b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
{
ex.x = a11; ex.y = a21;
ey.x = a12; ey.y = a22;
}
/// Initialize this matrix using columns.
void Set(const b2Vec2& c1, const b2Vec2& c2)
{
ex = c1;
ey = c2;
}
/// Set this to the identity matrix.
void SetIdentity()
{
ex.x = 1.0f; ey.x = 0.0f;
ex.y = 0.0f; ey.y = 1.0f;
}
/// Set this matrix to all zeros.
void SetZero()
{
ex.x = 0.0f; ey.x = 0.0f;
ex.y = 0.0f; ey.y = 0.0f;
}
b2Mat22 GetInverse() const
{
float32 a = ex.x, b = ey.x, c = ex.y, d = ey.y;
b2Mat22 B;
float32 det = a * d - b * c;
if (det != 0.0f)
{
det = 1.0f / det;
}
B.ex.x = det * d; B.ey.x = -det * b;
B.ex.y = -det * c; B.ey.y = det * a;
return B;
}
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
b2Vec2 Solve(const b2Vec2& b) const
{
float32 a11 = ex.x, a12 = ey.x, a21 = ex.y, a22 = ey.y;
float32 det = a11 * a22 - a12 * a21;
if (det != 0.0f)
{
det = 1.0f / det;
}
b2Vec2 x;
x.x = det * (a22 * b.x - a12 * b.y);
x.y = det * (a11 * b.y - a21 * b.x);
return x;
}
b2Vec2 ex, ey;
};
/// A 3-by-3 matrix. Stored in column-major order.
struct b2Mat33
{
/// The default constructor does nothing (for performance).
b2Mat33() {}
/// Construct this matrix using columns.
b2Mat33(const b2Vec3& c1, const b2Vec3& c2, const b2Vec3& c3)
{
ex = c1;
ey = c2;
ez = c3;
}
/// Set this matrix to all zeros.
void SetZero()
{
ex.SetZero();
ey.SetZero();
ez.SetZero();
}
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases.
b2Vec3 Solve33(const b2Vec3& b) const;
/// Solve A * x = b, where b is a column vector. This is more efficient
/// than computing the inverse in one-shot cases. Solve only the upper
/// 2-by-2 matrix equation.
b2Vec2 Solve22(const b2Vec2& b) const;
/// Get the inverse of this matrix as a 2-by-2.
/// Returns the zero matrix if singular.
void GetInverse22(b2Mat33* M) const;
/// Get the symmetric inverse of this matrix as a 3-by-3.
/// Returns the zero matrix if singular.
void GetSymInverse33(b2Mat33* M) const;
b2Vec3 ex, ey, ez;
};
/// Rotation
struct b2Rot
{
b2Rot() {}
/// Initialize from an angle in radians
explicit b2Rot(float32 angle)
{
/// TODO_ERIN optimize
s = sinf(angle);
c = cosf(angle);
}
/// Set using an angle in radians.
void Set(float32 angle)
{
/// TODO_ERIN optimize
s = sinf(angle);
c = cosf(angle);
}
/// Set to the identity rotation
void SetIdentity()
{
s = 0.0f;
c = 1.0f;
}
/// Get the angle in radians
float32 GetAngle() const
{
return b2Atan2(s, c);
}
/// Get the x-axis
b2Vec2 GetXAxis() const
{
return b2Vec2(c, s);
}
/// Get the u-axis
b2Vec2 GetYAxis() const
{
return b2Vec2(-s, c);
}
/// Sine and cosine
float32 s, c;
};
/// A transform contains translation and rotation. It is used to represent
/// the position and orientation of rigid frames.
struct b2Transform
{
/// The default constructor does nothing.
b2Transform() {}
/// Initialize using a position vector and a rotation.
b2Transform(const b2Vec2& position, const b2Rot& rotation) : p(position), q(rotation) {}
/// Set this to the identity transform.
void SetIdentity()
{
p.SetZero();
q.SetIdentity();
}
/// Set this based on the position and angle.
void Set(const b2Vec2& position, float32 angle)
{
p = position;
q.Set(angle);
}
b2Vec2 p;
b2Rot q;
};
/// This describes the motion of a body/shape for TOI computation.
/// Shapes are defined with respect to the body origin, which may
/// no coincide with the center of mass. However, to support dynamics
/// we must interpolate the center of mass position.
struct b2Sweep
{
/// Get the interpolated transform at a specific time.
/// @param beta is a factor in [0,1], where 0 indicates alpha0.
void GetTransform(b2Transform* xfb, float32 beta) const;
/// Advance the sweep forward, yielding a new initial state.
/// @param alpha the new initial time.
void Advance(float32 alpha);
/// Normalize the angles.
void Normalize();
b2Vec2 localCenter; ///< local center of mass position
b2Vec2 c0, c; ///< center world positions
float32 a0, a; ///< world angles
/// Fraction of the current time step in the range [0,1]
/// c0 and a0 are the positions at alpha0.
float32 alpha0;
};
/// Useful constant
extern const b2Vec2 b2Vec2_zero;
/// Perform the dot product on two vectors.
inline float32 b2Dot(const b2Vec2& a, const b2Vec2& b)
{
return a.x * b.x + a.y * b.y;
}
/// Perform the cross product on two vectors. In 2D this produces a scalar.
inline float32 b2Cross(const b2Vec2& a, const b2Vec2& b)
{
return a.x * b.y - a.y * b.x;
}
/// Perform the cross product on a vector and a scalar. In 2D this produces
/// a vector.
inline b2Vec2 b2Cross(const b2Vec2& a, float32 s)
{
return b2Vec2(s * a.y, -s * a.x);
}
/// Perform the cross product on a scalar and a vector. In 2D this produces
/// a vector.
inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
{
return b2Vec2(-s * a.y, s * a.x);
}
/// Multiply a matrix times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another.
inline b2Vec2 b2Mul(const b2Mat22& A, const b2Vec2& v)
{
return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
}
/// Multiply a matrix transpose times a vector. If a rotation matrix is provided,
/// then this transforms the vector from one frame to another (inverse transform).
inline b2Vec2 b2MulT(const b2Mat22& A, const b2Vec2& v)
{
return b2Vec2(b2Dot(v, A.ex), b2Dot(v, A.ey));
}
/// Add two vectors component-wise.
inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
{
return b2Vec2(a.x + b.x, a.y + b.y);
}
/// Subtract two vectors component-wise.
inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
{
return b2Vec2(a.x - b.x, a.y - b.y);
}
inline b2Vec2 operator * (float32 s, const b2Vec2& a)
{
return b2Vec2(s * a.x, s * a.y);
}
inline bool operator == (const b2Vec2& a, const b2Vec2& b)
{
return a.x == b.x && a.y == b.y;
}
inline float32 b2Distance(const b2Vec2& a, const b2Vec2& b)
{
b2Vec2 c = a - b;
return c.Length();
}
inline float32 b2DistanceSquared(const b2Vec2& a, const b2Vec2& b)
{
b2Vec2 c = a - b;
return b2Dot(c, c);
}
inline b2Vec3 operator * (float32 s, const b2Vec3& a)
{
return b2Vec3(s * a.x, s * a.y, s * a.z);
}
/// Add two vectors component-wise.
inline b2Vec3 operator + (const b2Vec3& a, const b2Vec3& b)
{
return b2Vec3(a.x + b.x, a.y + b.y, a.z + b.z);
}
/// Subtract two vectors component-wise.
inline b2Vec3 operator - (const b2Vec3& a, const b2Vec3& b)
{
return b2Vec3(a.x - b.x, a.y - b.y, a.z - b.z);
}
/// Perform the dot product on two vectors.
inline float32 b2Dot(const b2Vec3& a, const b2Vec3& b)
{
return a.x * b.x + a.y * b.y + a.z * b.z;
}
/// Perform the cross product on two vectors.
inline b2Vec3 b2Cross(const b2Vec3& a, const b2Vec3& b)
{
return b2Vec3(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}
inline b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
{
return b2Mat22(A.ex + B.ex, A.ey + B.ey);
}
// A * B
inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
{
return b2Mat22(b2Mul(A, B.ex), b2Mul(A, B.ey));
}
// A^T * B
inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
{
b2Vec2 c1(b2Dot(A.ex, B.ex), b2Dot(A.ey, B.ex));
b2Vec2 c2(b2Dot(A.ex, B.ey), b2Dot(A.ey, B.ey));
return b2Mat22(c1, c2);
}
/// Multiply a matrix times a vector.
inline b2Vec3 b2Mul(const b2Mat33& A, const b2Vec3& v)
{
return v.x * A.ex + v.y * A.ey + v.z * A.ez;
}
/// Multiply a matrix times a vector.
inline b2Vec2 b2Mul22(const b2Mat33& A, const b2Vec2& v)
{
return b2Vec2(A.ex.x * v.x + A.ey.x * v.y, A.ex.y * v.x + A.ey.y * v.y);
}
/// Multiply two rotations: q * r
inline b2Rot b2Mul(const b2Rot& q, const b2Rot& r)
{
// [qc -qs] * [rc -rs] = [qc*rc-qs*rs -qc*rs-qs*rc]
// [qs qc] [rs rc] [qs*rc+qc*rs -qs*rs+qc*rc]
// s = qs * rc + qc * rs
// c = qc * rc - qs * rs
b2Rot qr;
qr.s = q.s * r.c + q.c * r.s;
qr.c = q.c * r.c - q.s * r.s;
return qr;
}
/// Transpose multiply two rotations: qT * r
inline b2Rot b2MulT(const b2Rot& q, const b2Rot& r)
{
// [ qc qs] * [rc -rs] = [qc*rc+qs*rs -qc*rs+qs*rc]
// [-qs qc] [rs rc] [-qs*rc+qc*rs qs*rs+qc*rc]
// s = qc * rs - qs * rc
// c = qc * rc + qs * rs
b2Rot qr;
qr.s = q.c * r.s - q.s * r.c;
qr.c = q.c * r.c + q.s * r.s;
return qr;
}
/// Rotate a vector
inline b2Vec2 b2Mul(const b2Rot& q, const b2Vec2& v)
{
return b2Vec2(q.c * v.x - q.s * v.y, q.s * v.x + q.c * v.y);
}
/// Inverse rotate a vector
inline b2Vec2 b2MulT(const b2Rot& q, const b2Vec2& v)
{
return b2Vec2(q.c * v.x + q.s * v.y, -q.s * v.x + q.c * v.y);
}
inline b2Vec2 b2Mul(const b2Transform& T, const b2Vec2& v)
{
float32 x = (T.q.c * v.x - T.q.s * v.y) + T.p.x;
float32 y = (T.q.s * v.x + T.q.c * v.y) + T.p.y;
return b2Vec2(x, y);
}
inline b2Vec2 b2MulT(const b2Transform& T, const b2Vec2& v)
{
float32 px = v.x - T.p.x;
float32 py = v.y - T.p.y;
float32 x = (T.q.c * px + T.q.s * py);
float32 y = (-T.q.s * px + T.q.c * py);
return b2Vec2(x, y);
}
// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p
// = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p
inline b2Transform b2Mul(const b2Transform& A, const b2Transform& B)
{
b2Transform C;
C.q = b2Mul(A.q, B.q);
C.p = b2Mul(A.q, B.p) + A.p;
return C;
}
// v2 = A.q' * (B.q * v1 + B.p - A.p)
// = A.q' * B.q * v1 + A.q' * (B.p - A.p)
inline b2Transform b2MulT(const b2Transform& A, const b2Transform& B)
{
b2Transform C;
C.q = b2MulT(A.q, B.q);
C.p = b2MulT(A.q, B.p - A.p);
return C;
}
template <typename T>
inline T b2Abs(T a)
{
return a > T(0) ? a : -a;
}
inline b2Vec2 b2Abs(const b2Vec2& a)
{
return b2Vec2(b2Abs(a.x), b2Abs(a.y));
}
inline b2Mat22 b2Abs(const b2Mat22& A)
{
return b2Mat22(b2Abs(A.ex), b2Abs(A.ey));
}
template <typename T>
inline T b2Min(T a, T b)
{
return a < b ? a : b;
}
inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
{
return b2Vec2(b2Min(a.x, b.x), b2Min(a.y, b.y));
}
template <typename T>
inline T b2Max(T a, T b)
{
return a > b ? a : b;
}
inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
{
return b2Vec2(b2Max(a.x, b.x), b2Max(a.y, b.y));
}
template <typename T>
inline T b2Clamp(T a, T low, T high)
{
return b2Max(low, b2Min(a, high));
}
inline b2Vec2 b2Clamp(const b2Vec2& a, const b2Vec2& low, const b2Vec2& high)
{
return b2Max(low, b2Min(a, high));
}
template<typename T> inline void b2Swap(T& a, T& b)
{
T tmp = a;
a = b;
b = tmp;
}
/// "Next Largest Power of 2
/// Given a binary integer value x, the next largest power of 2 can be computed by a SWAR algorithm
/// that recursively "folds" the upper bits into the lower bits. This process yields a bit vector with
/// the same most significant 1 as x, but all 1's below it. Adding 1 to that value yields the next
/// largest power of 2. For a 32-bit value:"
inline uint32 b2NextPowerOfTwo(uint32 x)
{
x |= (x >> 1);
x |= (x >> 2);
x |= (x >> 4);
x |= (x >> 8);
x |= (x >> 16);
return x + 1;
}
inline bool b2IsPowerOfTwo(uint32 x)
{
bool result = x > 0 && (x & (x - 1)) == 0;
return result;
}
inline void b2Sweep::GetTransform(b2Transform* xf, float32 beta) const
{
xf->p = (1.0f - beta) * c0 + beta * c;
float32 angle = (1.0f - beta) * a0 + beta * a;
xf->q.Set(angle);
// Shift to origin
xf->p -= b2Mul(xf->q, localCenter);
}
inline void b2Sweep::Advance(float32 alpha)
{
b2Assert(alpha0 < 1.0f);
float32 beta = (alpha - alpha0) / (1.0f - alpha0);
c0 += beta * (c - c0);
a0 += beta * (a - a0);
alpha0 = alpha;
}
/// Normalize an angle in radians to be between -pi and pi
inline void b2Sweep::Normalize()
{
float32 twoPi = 2.0f * b2_pi;
float32 d = twoPi * floorf(a0 / twoPi);
a0 -= d;
a -= d;
}
#endif
+52
View File
@@ -0,0 +1,52 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Common/b2Settings.h>
#include <stdio.h>
#include <stdarg.h>
#include <stdlib.h>
#include "common/Exception.h"
b2Version b2_version = {2, 3, 0};
// Memory allocators. Modify these to use your own allocator.
void* b2Alloc(int32 size)
{
return malloc(size);
}
void b2Free(void* mem)
{
free(mem);
}
// You can modify this to use your logging facility.
void b2Log(const char* string, ...)
{
va_list args;
va_start(args, string);
vprintf(string, args);
va_end(args);
}
void loveAssert(bool test, const char *teststr)
{
if (!test)
throw love::Exception("Box2D assertion failed: %s", teststr);
}
+154
View File
@@ -0,0 +1,154 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_SETTINGS_H
#define B2_SETTINGS_H
#include <stddef.h>
#include <assert.h>
#include <float.h>
void loveAssert(bool test, const char *teststr);
#define B2_NOT_USED(x) ((void)(x))
//#define b2Assert(A) assert(A)
#define b2Assert(A) loveAssert((A), #A)
typedef signed char int8;
typedef signed short int16;
typedef signed int int32;
typedef unsigned char uint8;
typedef unsigned short uint16;
typedef unsigned int uint32;
typedef float float32;
typedef double float64;
#define b2_maxFloat FLT_MAX
#define b2_epsilon FLT_EPSILON
#define b2_pi 3.14159265359f
/// @file
/// Global tuning constants based on meters-kilograms-seconds (MKS) units.
///
// Collision
/// The maximum number of contact points between two convex shapes. Do
/// not change this value.
#define b2_maxManifoldPoints 2
/// The maximum number of vertices on a convex polygon. You cannot increase
/// this too much because b2BlockAllocator has a maximum object size.
#define b2_maxPolygonVertices 8
/// This is used to fatten AABBs in the dynamic tree. This allows proxies
/// to move by a small amount without triggering a tree adjustment.
/// This is in meters.
#define b2_aabbExtension 0.1f
/// This is used to fatten AABBs in the dynamic tree. This is used to predict
/// the future position based on the current displacement.
/// This is a dimensionless multiplier.
#define b2_aabbMultiplier 2.0f
/// A small length used as a collision and constraint tolerance. Usually it is
/// chosen to be numerically significant, but visually insignificant.
#define b2_linearSlop 0.005f
/// A small angle used as a collision and constraint tolerance. Usually it is
/// chosen to be numerically significant, but visually insignificant.
#define b2_angularSlop (2.0f / 180.0f * b2_pi)
/// The radius of the polygon/edge shape skin. This should not be modified. Making
/// this smaller means polygons will have an insufficient buffer for continuous collision.
/// Making it larger may create artifacts for vertex collision.
#define b2_polygonRadius (2.0f * b2_linearSlop)
/// Maximum number of sub-steps per contact in continuous physics simulation.
#define b2_maxSubSteps 8
// Dynamics
/// Maximum number of contacts to be handled to solve a TOI impact.
#define b2_maxTOIContacts 32
/// A velocity threshold for elastic collisions. Any collision with a relative linear
/// velocity below this threshold will be treated as inelastic.
#define b2_velocityThreshold 1.0f
/// The maximum linear position correction used when solving constraints. This helps to
/// prevent overshoot.
#define b2_maxLinearCorrection 0.2f
/// The maximum angular position correction used when solving constraints. This helps to
/// prevent overshoot.
#define b2_maxAngularCorrection (8.0f / 180.0f * b2_pi)
/// The maximum linear velocity of a body. This limit is very large and is used
/// to prevent numerical problems. You shouldn't need to adjust this.
#define b2_maxTranslation 2.0f
#define b2_maxTranslationSquared (b2_maxTranslation * b2_maxTranslation)
/// The maximum angular velocity of a body. This limit is very large and is used
/// to prevent numerical problems. You shouldn't need to adjust this.
#define b2_maxRotation (0.5f * b2_pi)
#define b2_maxRotationSquared (b2_maxRotation * b2_maxRotation)
/// This scale factor controls how fast overlap is resolved. Ideally this would be 1 so
/// that overlap is removed in one time step. However using values close to 1 often lead
/// to overshoot.
#define b2_baumgarte 0.2f
#define b2_toiBaugarte 0.75f
// Sleep
/// The time that a body must be still before it will go to sleep.
#define b2_timeToSleep 0.5f
/// A body cannot sleep if its linear velocity is above this tolerance.
#define b2_linearSleepTolerance 0.01f
/// A body cannot sleep if its angular velocity is above this tolerance.
#define b2_angularSleepTolerance (2.0f / 180.0f * b2_pi)
// Memory Allocation
/// Implement this function to use your own memory allocator.
void* b2Alloc(int32 size);
/// If you implement b2Alloc, you should also implement this function.
void b2Free(void* mem);
/// Logging function.
void b2Log(const char* string, ...);
/// Version numbering scheme.
/// See http://en.wikipedia.org/wiki/Software_versioning
struct b2Version
{
int32 major; ///< significant changes
int32 minor; ///< incremental changes
int32 revision; ///< bug fixes
};
/// Current version.
extern b2Version b2_version;
#endif
+83
View File
@@ -0,0 +1,83 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Common/b2StackAllocator.h>
#include <Box2D/Common/b2Math.h>
b2StackAllocator::b2StackAllocator()
{
m_index = 0;
m_allocation = 0;
m_maxAllocation = 0;
m_entryCount = 0;
}
b2StackAllocator::~b2StackAllocator()
{
b2Assert(m_index == 0);
b2Assert(m_entryCount == 0);
}
void* b2StackAllocator::Allocate(int32 size)
{
b2Assert(m_entryCount < b2_maxStackEntries);
b2StackEntry* entry = m_entries + m_entryCount;
entry->size = size;
if (m_index + size > b2_stackSize)
{
entry->data = (char*)b2Alloc(size);
entry->usedMalloc = true;
}
else
{
entry->data = m_data + m_index;
entry->usedMalloc = false;
m_index += size;
}
m_allocation += size;
m_maxAllocation = b2Max(m_maxAllocation, m_allocation);
++m_entryCount;
return entry->data;
}
void b2StackAllocator::Free(void* p)
{
b2Assert(m_entryCount > 0);
b2StackEntry* entry = m_entries + m_entryCount - 1;
b2Assert(p == entry->data);
if (entry->usedMalloc)
{
b2Free(p);
}
else
{
m_index -= entry->size;
}
m_allocation -= entry->size;
--m_entryCount;
p = NULL;
}
int32 b2StackAllocator::GetMaxAllocation() const
{
return m_maxAllocation;
}
+60
View File
@@ -0,0 +1,60 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_STACK_ALLOCATOR_H
#define B2_STACK_ALLOCATOR_H
#include <Box2D/Common/b2Settings.h>
const int32 b2_stackSize = 100 * 1024; // 100k
const int32 b2_maxStackEntries = 32;
struct b2StackEntry
{
char* data;
int32 size;
bool usedMalloc;
};
// This is a stack allocator used for fast per step allocations.
// You must nest allocate/free pairs. The code will assert
// if you try to interleave multiple allocate/free pairs.
class b2StackAllocator
{
public:
b2StackAllocator();
~b2StackAllocator();
void* Allocate(int32 size);
void Free(void* p);
int32 GetMaxAllocation() const;
private:
char m_data[b2_stackSize];
int32 m_index;
int32 m_allocation;
int32 m_maxAllocation;
b2StackEntry m_entries[b2_maxStackEntries];
int32 m_entryCount;
};
#endif
+101
View File
@@ -0,0 +1,101 @@
/*
* Copyright (c) 2011 Erin Catto http://box2d.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 <Box2D/Common/b2Timer.h>
#if defined(_WIN32)
float64 b2Timer::s_invFrequency = 0.0f;
#define WIN32_LEAN_AND_MEAN
#include <windows.h>
b2Timer::b2Timer()
{
LARGE_INTEGER largeInteger;
if (s_invFrequency == 0.0f)
{
QueryPerformanceFrequency(&largeInteger);
s_invFrequency = float64(largeInteger.QuadPart);
if (s_invFrequency > 0.0f)
{
s_invFrequency = 1000.0f / s_invFrequency;
}
}
QueryPerformanceCounter(&largeInteger);
m_start = float64(largeInteger.QuadPart);
}
void b2Timer::Reset()
{
LARGE_INTEGER largeInteger;
QueryPerformanceCounter(&largeInteger);
m_start = float64(largeInteger.QuadPart);
}
float32 b2Timer::GetMilliseconds() const
{
LARGE_INTEGER largeInteger;
QueryPerformanceCounter(&largeInteger);
float64 count = float64(largeInteger.QuadPart);
float32 ms = float32(s_invFrequency * (count - m_start));
return ms;
}
#elif defined(__linux__) || defined (__APPLE__)
#include <sys/time.h>
b2Timer::b2Timer()
{
Reset();
}
void b2Timer::Reset()
{
timeval t;
gettimeofday(&t, 0);
m_start_sec = t.tv_sec;
m_start_usec = t.tv_usec;
}
float32 b2Timer::GetMilliseconds() const
{
timeval t;
gettimeofday(&t, 0);
return 1000.0f * (t.tv_sec - m_start_sec) + 0.001f * (t.tv_usec - m_start_usec);
}
#else
b2Timer::b2Timer()
{
}
void b2Timer::Reset()
{
}
float32 b2Timer::GetMilliseconds() const
{
return 0.0f;
}
#endif
+50
View File
@@ -0,0 +1,50 @@
/*
* Copyright (c) 2011 Erin Catto http://box2d.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 B2_TIMER_H
#define B2_TIMER_H
#include <Box2D/Common/b2Settings.h>
/// Timer for profiling. This has platform specific code and may
/// not work on every platform.
class b2Timer
{
public:
/// Constructor
b2Timer();
/// Reset the timer.
void Reset();
/// Get the time since construction or the last reset.
float32 GetMilliseconds() const;
private:
#if defined(_WIN32)
float64 m_start;
static float64 s_invFrequency;
#elif defined(__linux__) || defined (__APPLE__)
unsigned long m_start_sec;
unsigned long m_start_usec;
#endif
};
#endif
@@ -0,0 +1,53 @@
/*
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h>
#include <Box2D/Common/b2BlockAllocator.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <Box2D/Collision/Shapes/b2ChainShape.h>
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
#include <new>
b2Contact* b2ChainAndCircleContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
{
void* mem = allocator->Allocate(sizeof(b2ChainAndCircleContact));
return new (mem) b2ChainAndCircleContact(fixtureA, indexA, fixtureB, indexB);
}
void b2ChainAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
{
((b2ChainAndCircleContact*)contact)->~b2ChainAndCircleContact();
allocator->Free(contact, sizeof(b2ChainAndCircleContact));
}
b2ChainAndCircleContact::b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
: b2Contact(fixtureA, indexA, fixtureB, indexB)
{
b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
}
void b2ChainAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
{
b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
b2EdgeShape edge;
chain->GetChildEdge(&edge, m_indexA);
b2CollideEdgeAndCircle( manifold, &edge, xfA,
(b2CircleShape*)m_fixtureB->GetShape(), xfB);
}
@@ -0,0 +1,39 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_CHAIN_AND_CIRCLE_CONTACT_H
#define B2_CHAIN_AND_CIRCLE_CONTACT_H
#include <Box2D/Dynamics/Contacts/b2Contact.h>
class b2BlockAllocator;
class b2ChainAndCircleContact : public b2Contact
{
public:
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
b2ChainAndCircleContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
~b2ChainAndCircleContact() {}
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
};
#endif
@@ -0,0 +1,53 @@
/*
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h>
#include <Box2D/Common/b2BlockAllocator.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <Box2D/Collision/Shapes/b2ChainShape.h>
#include <Box2D/Collision/Shapes/b2EdgeShape.h>
#include <new>
b2Contact* b2ChainAndPolygonContact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
{
void* mem = allocator->Allocate(sizeof(b2ChainAndPolygonContact));
return new (mem) b2ChainAndPolygonContact(fixtureA, indexA, fixtureB, indexB);
}
void b2ChainAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
{
((b2ChainAndPolygonContact*)contact)->~b2ChainAndPolygonContact();
allocator->Free(contact, sizeof(b2ChainAndPolygonContact));
}
b2ChainAndPolygonContact::b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB)
: b2Contact(fixtureA, indexA, fixtureB, indexB)
{
b2Assert(m_fixtureA->GetType() == b2Shape::e_chain);
b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
}
void b2ChainAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
{
b2ChainShape* chain = (b2ChainShape*)m_fixtureA->GetShape();
b2EdgeShape edge;
chain->GetChildEdge(&edge, m_indexA);
b2CollideEdgeAndPolygon( manifold, &edge, xfA,
(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
}
@@ -0,0 +1,39 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_CHAIN_AND_POLYGON_CONTACT_H
#define B2_CHAIN_AND_POLYGON_CONTACT_H
#include <Box2D/Dynamics/Contacts/b2Contact.h>
class b2BlockAllocator;
class b2ChainAndPolygonContact : public b2Contact
{
public:
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
b2ChainAndPolygonContact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
~b2ChainAndPolygonContact() {}
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
};
#endif
@@ -0,0 +1,52 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Contacts/b2CircleContact.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <Box2D/Dynamics/b2WorldCallbacks.h>
#include <Box2D/Common/b2BlockAllocator.h>
#include <Box2D/Collision/b2TimeOfImpact.h>
#include <new>
b2Contact* b2CircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
{
void* mem = allocator->Allocate(sizeof(b2CircleContact));
return new (mem) b2CircleContact(fixtureA, fixtureB);
}
void b2CircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
{
((b2CircleContact*)contact)->~b2CircleContact();
allocator->Free(contact, sizeof(b2CircleContact));
}
b2CircleContact::b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
: b2Contact(fixtureA, 0, fixtureB, 0)
{
b2Assert(m_fixtureA->GetType() == b2Shape::e_circle);
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
}
void b2CircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
{
b2CollideCircles(manifold,
(b2CircleShape*)m_fixtureA->GetShape(), xfA,
(b2CircleShape*)m_fixtureB->GetShape(), xfB);
}
@@ -0,0 +1,39 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_CIRCLE_CONTACT_H
#define B2_CIRCLE_CONTACT_H
#include <Box2D/Dynamics/Contacts/b2Contact.h>
class b2BlockAllocator;
class b2CircleContact : public b2Contact
{
public:
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
b2CircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
~b2CircleContact() {}
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
};
#endif
@@ -0,0 +1,247 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Contacts/b2Contact.h>
#include <Box2D/Dynamics/Contacts/b2CircleContact.h>
#include <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>
#include <Box2D/Dynamics/Contacts/b2PolygonContact.h>
#include <Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h>
#include <Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h>
#include <Box2D/Dynamics/Contacts/b2ChainAndCircleContact.h>
#include <Box2D/Dynamics/Contacts/b2ChainAndPolygonContact.h>
#include <Box2D/Dynamics/Contacts/b2ContactSolver.h>
#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/b2TimeOfImpact.h>
#include <Box2D/Collision/Shapes/b2Shape.h>
#include <Box2D/Common/b2BlockAllocator.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <Box2D/Dynamics/b2World.h>
b2ContactRegister b2Contact::s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
bool b2Contact::s_initialized = false;
void b2Contact::InitializeRegisters()
{
AddType(b2CircleContact::Create, b2CircleContact::Destroy, b2Shape::e_circle, b2Shape::e_circle);
AddType(b2PolygonAndCircleContact::Create, b2PolygonAndCircleContact::Destroy, b2Shape::e_polygon, b2Shape::e_circle);
AddType(b2PolygonContact::Create, b2PolygonContact::Destroy, b2Shape::e_polygon, b2Shape::e_polygon);
AddType(b2EdgeAndCircleContact::Create, b2EdgeAndCircleContact::Destroy, b2Shape::e_edge, b2Shape::e_circle);
AddType(b2EdgeAndPolygonContact::Create, b2EdgeAndPolygonContact::Destroy, b2Shape::e_edge, b2Shape::e_polygon);
AddType(b2ChainAndCircleContact::Create, b2ChainAndCircleContact::Destroy, b2Shape::e_chain, b2Shape::e_circle);
AddType(b2ChainAndPolygonContact::Create, b2ChainAndPolygonContact::Destroy, b2Shape::e_chain, b2Shape::e_polygon);
}
void b2Contact::AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destoryFcn,
b2Shape::Type type1, b2Shape::Type type2)
{
b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
s_registers[type1][type2].createFcn = createFcn;
s_registers[type1][type2].destroyFcn = destoryFcn;
s_registers[type1][type2].primary = true;
if (type1 != type2)
{
s_registers[type2][type1].createFcn = createFcn;
s_registers[type2][type1].destroyFcn = destoryFcn;
s_registers[type2][type1].primary = false;
}
}
b2Contact* b2Contact::Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator)
{
if (s_initialized == false)
{
InitializeRegisters();
s_initialized = true;
}
b2Shape::Type type1 = fixtureA->GetType();
b2Shape::Type type2 = fixtureB->GetType();
b2Assert(0 <= type1 && type1 < b2Shape::e_typeCount);
b2Assert(0 <= type2 && type2 < b2Shape::e_typeCount);
b2ContactCreateFcn* createFcn = s_registers[type1][type2].createFcn;
if (createFcn)
{
if (s_registers[type1][type2].primary)
{
return createFcn(fixtureA, indexA, fixtureB, indexB, allocator);
}
else
{
return createFcn(fixtureB, indexB, fixtureA, indexA, allocator);
}
}
else
{
return NULL;
}
}
void b2Contact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
{
b2Assert(s_initialized == true);
b2Fixture* fixtureA = contact->m_fixtureA;
b2Fixture* fixtureB = contact->m_fixtureB;
if (contact->m_manifold.pointCount > 0 &&
fixtureA->IsSensor() == false &&
fixtureB->IsSensor() == false)
{
fixtureA->GetBody()->SetAwake(true);
fixtureB->GetBody()->SetAwake(true);
}
b2Shape::Type typeA = fixtureA->GetType();
b2Shape::Type typeB = fixtureB->GetType();
b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
b2Assert(0 <= typeA && typeB < b2Shape::e_typeCount);
b2ContactDestroyFcn* destroyFcn = s_registers[typeA][typeB].destroyFcn;
destroyFcn(contact, allocator);
}
b2Contact::b2Contact(b2Fixture* fA, int32 indexA, b2Fixture* fB, int32 indexB)
{
m_flags = e_enabledFlag;
m_fixtureA = fA;
m_fixtureB = fB;
m_indexA = indexA;
m_indexB = indexB;
m_manifold.pointCount = 0;
m_prev = NULL;
m_next = NULL;
m_nodeA.contact = NULL;
m_nodeA.prev = NULL;
m_nodeA.next = NULL;
m_nodeA.other = NULL;
m_nodeB.contact = NULL;
m_nodeB.prev = NULL;
m_nodeB.next = NULL;
m_nodeB.other = NULL;
m_toiCount = 0;
m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
m_tangentSpeed = 0.0f;
}
// Update the contact manifold and touching status.
// Note: do not assume the fixture AABBs are overlapping or are valid.
void b2Contact::Update(b2ContactListener* listener)
{
b2Manifold oldManifold = m_manifold;
// Re-enable this contact.
m_flags |= e_enabledFlag;
bool touching = false;
bool wasTouching = (m_flags & e_touchingFlag) == e_touchingFlag;
bool sensorA = m_fixtureA->IsSensor();
bool sensorB = m_fixtureB->IsSensor();
bool sensor = sensorA || sensorB;
b2Body* bodyA = m_fixtureA->GetBody();
b2Body* bodyB = m_fixtureB->GetBody();
const b2Transform& xfA = bodyA->GetTransform();
const b2Transform& xfB = bodyB->GetTransform();
// Is this contact a sensor?
if (sensor)
{
const b2Shape* shapeA = m_fixtureA->GetShape();
const b2Shape* shapeB = m_fixtureB->GetShape();
touching = b2TestOverlap(shapeA, m_indexA, shapeB, m_indexB, xfA, xfB);
// Sensors don't generate manifolds.
m_manifold.pointCount = 0;
}
else
{
Evaluate(&m_manifold, xfA, xfB);
touching = m_manifold.pointCount > 0;
// Match old contact ids to new contact ids and copy the
// stored impulses to warm start the solver.
for (int32 i = 0; i < m_manifold.pointCount; ++i)
{
b2ManifoldPoint* mp2 = m_manifold.points + i;
mp2->normalImpulse = 0.0f;
mp2->tangentImpulse = 0.0f;
b2ContactID id2 = mp2->id;
for (int32 j = 0; j < oldManifold.pointCount; ++j)
{
b2ManifoldPoint* mp1 = oldManifold.points + j;
if (mp1->id.key == id2.key)
{
mp2->normalImpulse = mp1->normalImpulse;
mp2->tangentImpulse = mp1->tangentImpulse;
break;
}
}
}
if (touching != wasTouching)
{
bodyA->SetAwake(true);
bodyB->SetAwake(true);
}
}
if (touching)
{
m_flags |= e_touchingFlag;
}
else
{
m_flags &= ~e_touchingFlag;
}
if (wasTouching == false && touching == true && listener)
{
listener->BeginContact(this);
}
if (wasTouching == true && touching == false && listener)
{
listener->EndContact(this);
}
if (sensor == false && touching && listener)
{
listener->PreSolve(this, &oldManifold);
}
}
+349
View File
@@ -0,0 +1,349 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_CONTACT_H
#define B2_CONTACT_H
#include <Box2D/Common/b2Math.h>
#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Collision/Shapes/b2Shape.h>
#include <Box2D/Dynamics/b2Fixture.h>
class b2Body;
class b2Contact;
class b2Fixture;
class b2World;
class b2BlockAllocator;
class b2StackAllocator;
class b2ContactListener;
/// Friction mixing law. The idea is to allow either fixture to drive the restitution to zero.
/// For example, anything slides on ice.
inline float32 b2MixFriction(float32 friction1, float32 friction2)
{
return b2Sqrt(friction1 * friction2);
}
/// Restitution mixing law. The idea is allow for anything to bounce off an inelastic surface.
/// For example, a superball bounces on anything.
inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)
{
return restitution1 > restitution2 ? restitution1 : restitution2;
}
typedef b2Contact* b2ContactCreateFcn( b2Fixture* fixtureA, int32 indexA,
b2Fixture* fixtureB, int32 indexB,
b2BlockAllocator* allocator);
typedef void b2ContactDestroyFcn(b2Contact* contact, b2BlockAllocator* allocator);
struct b2ContactRegister
{
b2ContactCreateFcn* createFcn;
b2ContactDestroyFcn* destroyFcn;
bool primary;
};
/// A contact edge is used to connect bodies and contacts together
/// in a contact graph where each body is a node and each contact
/// is an edge. A contact edge belongs to a doubly linked list
/// maintained in each attached body. Each contact has two contact
/// nodes, one for each attached body.
struct b2ContactEdge
{
b2Body* other; ///< provides quick access to the other body attached.
b2Contact* contact; ///< the contact
b2ContactEdge* prev; ///< the previous contact edge in the body's contact list
b2ContactEdge* next; ///< the next contact edge in the body's contact list
};
/// The class manages contact between two shapes. A contact exists for each overlapping
/// AABB in the broad-phase (except if filtered). Therefore a contact object may exist
/// that has no contact points.
class b2Contact
{
public:
/// Get the contact manifold. Do not modify the manifold unless you understand the
/// internals of Box2D.
b2Manifold* GetManifold();
const b2Manifold* GetManifold() const;
/// Get the world manifold.
void GetWorldManifold(b2WorldManifold* worldManifold) const;
/// Is this contact touching?
bool IsTouching() const;
/// Enable/disable this contact. This can be used inside the pre-solve
/// contact listener. The contact is only disabled for the current
/// time step (or sub-step in continuous collisions).
void SetEnabled(bool flag);
/// Has this contact been disabled?
bool IsEnabled() const;
/// Get the next contact in the world's contact list.
b2Contact* GetNext();
const b2Contact* GetNext() const;
/// Get fixture A in this contact.
b2Fixture* GetFixtureA();
const b2Fixture* GetFixtureA() const;
/// Get the child primitive index for fixture A.
int32 GetChildIndexA() const;
/// Get fixture B in this contact.
b2Fixture* GetFixtureB();
const b2Fixture* GetFixtureB() const;
/// Get the child primitive index for fixture B.
int32 GetChildIndexB() const;
/// Override the default friction mixture. You can call this in b2ContactListener::PreSolve.
/// This value persists until set or reset.
void SetFriction(float32 friction);
/// Get the friction.
float32 GetFriction() const;
/// Reset the friction mixture to the default value.
void ResetFriction();
/// Override the default restitution mixture. You can call this in b2ContactListener::PreSolve.
/// The value persists until you set or reset.
void SetRestitution(float32 restitution);
/// Get the restitution.
float32 GetRestitution() const;
/// Reset the restitution to the default value.
void ResetRestitution();
/// Set the desired tangent speed for a conveyor belt behavior. In meters per second.
void SetTangentSpeed(float32 speed);
/// Get the desired tangent speed. In meters per second.
float32 GetTangentSpeed() const;
/// Evaluate this contact with your own manifold and transforms.
virtual void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) = 0;
protected:
friend class b2ContactManager;
friend class b2World;
friend class b2ContactSolver;
friend class b2Body;
friend class b2Fixture;
// Flags stored in m_flags
enum
{
// Used when crawling contact graph when forming islands.
e_islandFlag = 0x0001,
// Set when the shapes are touching.
e_touchingFlag = 0x0002,
// This contact can be disabled (by user)
e_enabledFlag = 0x0004,
// This contact needs filtering because a fixture filter was changed.
e_filterFlag = 0x0008,
// This bullet contact had a TOI event
e_bulletHitFlag = 0x0010,
// This contact has a valid TOI in m_toi
e_toiFlag = 0x0020
};
/// Flag this contact for filtering. Filtering will occur the next time step.
void FlagForFiltering();
static void AddType(b2ContactCreateFcn* createFcn, b2ContactDestroyFcn* destroyFcn,
b2Shape::Type typeA, b2Shape::Type typeB);
static void InitializeRegisters();
static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
static void Destroy(b2Contact* contact, b2Shape::Type typeA, b2Shape::Type typeB, b2BlockAllocator* allocator);
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
b2Contact() : m_fixtureA(NULL), m_fixtureB(NULL) {}
b2Contact(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB);
virtual ~b2Contact() {}
void Update(b2ContactListener* listener);
static b2ContactRegister s_registers[b2Shape::e_typeCount][b2Shape::e_typeCount];
static bool s_initialized;
uint32 m_flags;
// World pool and list pointers.
b2Contact* m_prev;
b2Contact* m_next;
// Nodes for connecting bodies.
b2ContactEdge m_nodeA;
b2ContactEdge m_nodeB;
b2Fixture* m_fixtureA;
b2Fixture* m_fixtureB;
int32 m_indexA;
int32 m_indexB;
b2Manifold m_manifold;
int32 m_toiCount;
float32 m_toi;
float32 m_friction;
float32 m_restitution;
float32 m_tangentSpeed;
};
inline b2Manifold* b2Contact::GetManifold()
{
return &m_manifold;
}
inline const b2Manifold* b2Contact::GetManifold() const
{
return &m_manifold;
}
inline void b2Contact::GetWorldManifold(b2WorldManifold* worldManifold) const
{
const b2Body* bodyA = m_fixtureA->GetBody();
const b2Body* bodyB = m_fixtureB->GetBody();
const b2Shape* shapeA = m_fixtureA->GetShape();
const b2Shape* shapeB = m_fixtureB->GetShape();
worldManifold->Initialize(&m_manifold, bodyA->GetTransform(), shapeA->m_radius, bodyB->GetTransform(), shapeB->m_radius);
}
inline void b2Contact::SetEnabled(bool flag)
{
if (flag)
{
m_flags |= e_enabledFlag;
}
else
{
m_flags &= ~e_enabledFlag;
}
}
inline bool b2Contact::IsEnabled() const
{
return (m_flags & e_enabledFlag) == e_enabledFlag;
}
inline bool b2Contact::IsTouching() const
{
return (m_flags & e_touchingFlag) == e_touchingFlag;
}
inline b2Contact* b2Contact::GetNext()
{
return m_next;
}
inline const b2Contact* b2Contact::GetNext() const
{
return m_next;
}
inline b2Fixture* b2Contact::GetFixtureA()
{
return m_fixtureA;
}
inline const b2Fixture* b2Contact::GetFixtureA() const
{
return m_fixtureA;
}
inline b2Fixture* b2Contact::GetFixtureB()
{
return m_fixtureB;
}
inline int32 b2Contact::GetChildIndexA() const
{
return m_indexA;
}
inline const b2Fixture* b2Contact::GetFixtureB() const
{
return m_fixtureB;
}
inline int32 b2Contact::GetChildIndexB() const
{
return m_indexB;
}
inline void b2Contact::FlagForFiltering()
{
m_flags |= e_filterFlag;
}
inline void b2Contact::SetFriction(float32 friction)
{
m_friction = friction;
}
inline float32 b2Contact::GetFriction() const
{
return m_friction;
}
inline void b2Contact::ResetFriction()
{
m_friction = b2MixFriction(m_fixtureA->m_friction, m_fixtureB->m_friction);
}
inline void b2Contact::SetRestitution(float32 restitution)
{
m_restitution = restitution;
}
inline float32 b2Contact::GetRestitution() const
{
return m_restitution;
}
inline void b2Contact::ResetRestitution()
{
m_restitution = b2MixRestitution(m_fixtureA->m_restitution, m_fixtureB->m_restitution);
}
inline void b2Contact::SetTangentSpeed(float32 speed)
{
m_tangentSpeed = speed;
}
inline float32 b2Contact::GetTangentSpeed() const
{
return m_tangentSpeed;
}
#endif
@@ -0,0 +1,833 @@
/*
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Contacts/b2ContactSolver.h>
#include <Box2D/Dynamics/Contacts/b2Contact.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <Box2D/Dynamics/b2World.h>
#include <Box2D/Common/b2StackAllocator.h>
#define B2_DEBUG_SOLVER 0
struct b2ContactPositionConstraint
{
b2Vec2 localPoints[b2_maxManifoldPoints];
b2Vec2 localNormal;
b2Vec2 localPoint;
int32 indexA;
int32 indexB;
float32 invMassA, invMassB;
b2Vec2 localCenterA, localCenterB;
float32 invIA, invIB;
b2Manifold::Type type;
float32 radiusA, radiusB;
int32 pointCount;
};
b2ContactSolver::b2ContactSolver(b2ContactSolverDef* def)
{
m_step = def->step;
m_allocator = def->allocator;
m_count = def->count;
m_positionConstraints = (b2ContactPositionConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactPositionConstraint));
m_velocityConstraints = (b2ContactVelocityConstraint*)m_allocator->Allocate(m_count * sizeof(b2ContactVelocityConstraint));
m_positions = def->positions;
m_velocities = def->velocities;
m_contacts = def->contacts;
// Initialize position independent portions of the constraints.
for (int32 i = 0; i < m_count; ++i)
{
b2Contact* contact = m_contacts[i];
b2Fixture* fixtureA = contact->m_fixtureA;
b2Fixture* fixtureB = contact->m_fixtureB;
b2Shape* shapeA = fixtureA->GetShape();
b2Shape* shapeB = fixtureB->GetShape();
float32 radiusA = shapeA->m_radius;
float32 radiusB = shapeB->m_radius;
b2Body* bodyA = fixtureA->GetBody();
b2Body* bodyB = fixtureB->GetBody();
b2Manifold* manifold = contact->GetManifold();
int32 pointCount = manifold->pointCount;
b2Assert(pointCount > 0);
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
vc->friction = contact->m_friction;
vc->restitution = contact->m_restitution;
vc->tangentSpeed = contact->m_tangentSpeed;
vc->indexA = bodyA->m_islandIndex;
vc->indexB = bodyB->m_islandIndex;
vc->invMassA = bodyA->m_invMass;
vc->invMassB = bodyB->m_invMass;
vc->invIA = bodyA->m_invI;
vc->invIB = bodyB->m_invI;
vc->contactIndex = i;
vc->pointCount = pointCount;
vc->K.SetZero();
vc->normalMass.SetZero();
b2ContactPositionConstraint* pc = m_positionConstraints + i;
pc->indexA = bodyA->m_islandIndex;
pc->indexB = bodyB->m_islandIndex;
pc->invMassA = bodyA->m_invMass;
pc->invMassB = bodyB->m_invMass;
pc->localCenterA = bodyA->m_sweep.localCenter;
pc->localCenterB = bodyB->m_sweep.localCenter;
pc->invIA = bodyA->m_invI;
pc->invIB = bodyB->m_invI;
pc->localNormal = manifold->localNormal;
pc->localPoint = manifold->localPoint;
pc->pointCount = pointCount;
pc->radiusA = radiusA;
pc->radiusB = radiusB;
pc->type = manifold->type;
for (int32 j = 0; j < pointCount; ++j)
{
b2ManifoldPoint* cp = manifold->points + j;
b2VelocityConstraintPoint* vcp = vc->points + j;
if (m_step.warmStarting)
{
vcp->normalImpulse = m_step.dtRatio * cp->normalImpulse;
vcp->tangentImpulse = m_step.dtRatio * cp->tangentImpulse;
}
else
{
vcp->normalImpulse = 0.0f;
vcp->tangentImpulse = 0.0f;
}
vcp->rA.SetZero();
vcp->rB.SetZero();
vcp->normalMass = 0.0f;
vcp->tangentMass = 0.0f;
vcp->velocityBias = 0.0f;
pc->localPoints[j] = cp->localPoint;
}
}
}
b2ContactSolver::~b2ContactSolver()
{
m_allocator->Free(m_velocityConstraints);
m_allocator->Free(m_positionConstraints);
}
// Initialize position dependent portions of the velocity constraints.
void b2ContactSolver::InitializeVelocityConstraints()
{
for (int32 i = 0; i < m_count; ++i)
{
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
b2ContactPositionConstraint* pc = m_positionConstraints + i;
float32 radiusA = pc->radiusA;
float32 radiusB = pc->radiusB;
b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
int32 indexA = vc->indexA;
int32 indexB = vc->indexB;
float32 mA = vc->invMassA;
float32 mB = vc->invMassB;
float32 iA = vc->invIA;
float32 iB = vc->invIB;
b2Vec2 localCenterA = pc->localCenterA;
b2Vec2 localCenterB = pc->localCenterB;
b2Vec2 cA = m_positions[indexA].c;
float32 aA = m_positions[indexA].a;
b2Vec2 vA = m_velocities[indexA].v;
float32 wA = m_velocities[indexA].w;
b2Vec2 cB = m_positions[indexB].c;
float32 aB = m_positions[indexB].a;
b2Vec2 vB = m_velocities[indexB].v;
float32 wB = m_velocities[indexB].w;
b2Assert(manifold->pointCount > 0);
b2Transform xfA, xfB;
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - b2Mul(xfA.q, localCenterA);
xfB.p = cB - b2Mul(xfB.q, localCenterB);
b2WorldManifold worldManifold;
worldManifold.Initialize(manifold, xfA, radiusA, xfB, radiusB);
vc->normal = worldManifold.normal;
int32 pointCount = vc->pointCount;
for (int32 j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint* vcp = vc->points + j;
vcp->rA = worldManifold.points[j] - cA;
vcp->rB = worldManifold.points[j] - cB;
float32 rnA = b2Cross(vcp->rA, vc->normal);
float32 rnB = b2Cross(vcp->rB, vc->normal);
float32 kNormal = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
vcp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f;
b2Vec2 tangent = b2Cross(vc->normal, 1.0f);
float32 rtA = b2Cross(vcp->rA, tangent);
float32 rtB = b2Cross(vcp->rB, tangent);
float32 kTangent = mA + mB + iA * rtA * rtA + iB * rtB * rtB;
vcp->tangentMass = kTangent > 0.0f ? 1.0f / kTangent : 0.0f;
// Setup a velocity bias for restitution.
vcp->velocityBias = 0.0f;
float32 vRel = b2Dot(vc->normal, vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA));
if (vRel < -b2_velocityThreshold)
{
vcp->velocityBias = -vc->restitution * vRel;
}
}
// If we have two points, then prepare the block solver.
if (vc->pointCount == 2)
{
b2VelocityConstraintPoint* vcp1 = vc->points + 0;
b2VelocityConstraintPoint* vcp2 = vc->points + 1;
float32 rn1A = b2Cross(vcp1->rA, vc->normal);
float32 rn1B = b2Cross(vcp1->rB, vc->normal);
float32 rn2A = b2Cross(vcp2->rA, vc->normal);
float32 rn2B = b2Cross(vcp2->rB, vc->normal);
float32 k11 = mA + mB + iA * rn1A * rn1A + iB * rn1B * rn1B;
float32 k22 = mA + mB + iA * rn2A * rn2A + iB * rn2B * rn2B;
float32 k12 = mA + mB + iA * rn1A * rn2A + iB * rn1B * rn2B;
// Ensure a reasonable condition number.
const float32 k_maxConditionNumber = 1000.0f;
if (k11 * k11 < k_maxConditionNumber * (k11 * k22 - k12 * k12))
{
// K is safe to invert.
vc->K.ex.Set(k11, k12);
vc->K.ey.Set(k12, k22);
vc->normalMass = vc->K.GetInverse();
}
else
{
// The constraints are redundant, just use one.
// TODO_ERIN use deepest?
vc->pointCount = 1;
}
}
}
}
void b2ContactSolver::WarmStart()
{
// Warm start.
for (int32 i = 0; i < m_count; ++i)
{
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
int32 indexA = vc->indexA;
int32 indexB = vc->indexB;
float32 mA = vc->invMassA;
float32 iA = vc->invIA;
float32 mB = vc->invMassB;
float32 iB = vc->invIB;
int32 pointCount = vc->pointCount;
b2Vec2 vA = m_velocities[indexA].v;
float32 wA = m_velocities[indexA].w;
b2Vec2 vB = m_velocities[indexB].v;
float32 wB = m_velocities[indexB].w;
b2Vec2 normal = vc->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
for (int32 j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint* vcp = vc->points + j;
b2Vec2 P = vcp->normalImpulse * normal + vcp->tangentImpulse * tangent;
wA -= iA * b2Cross(vcp->rA, P);
vA -= mA * P;
wB += iB * b2Cross(vcp->rB, P);
vB += mB * P;
}
m_velocities[indexA].v = vA;
m_velocities[indexA].w = wA;
m_velocities[indexB].v = vB;
m_velocities[indexB].w = wB;
}
}
void b2ContactSolver::SolveVelocityConstraints()
{
for (int32 i = 0; i < m_count; ++i)
{
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
int32 indexA = vc->indexA;
int32 indexB = vc->indexB;
float32 mA = vc->invMassA;
float32 iA = vc->invIA;
float32 mB = vc->invMassB;
float32 iB = vc->invIB;
int32 pointCount = vc->pointCount;
b2Vec2 vA = m_velocities[indexA].v;
float32 wA = m_velocities[indexA].w;
b2Vec2 vB = m_velocities[indexB].v;
float32 wB = m_velocities[indexB].w;
b2Vec2 normal = vc->normal;
b2Vec2 tangent = b2Cross(normal, 1.0f);
float32 friction = vc->friction;
b2Assert(pointCount == 1 || pointCount == 2);
// Solve tangent constraints first because non-penetration is more important
// than friction.
for (int32 j = 0; j < pointCount; ++j)
{
b2VelocityConstraintPoint* vcp = vc->points + j;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
// Compute tangent force
float32 vt = b2Dot(dv, tangent) - vc->tangentSpeed;
float32 lambda = vcp->tangentMass * (-vt);
// b2Clamp the accumulated force
float32 maxFriction = friction * vcp->normalImpulse;
float32 newImpulse = b2Clamp(vcp->tangentImpulse + lambda, -maxFriction, maxFriction);
lambda = newImpulse - vcp->tangentImpulse;
vcp->tangentImpulse = newImpulse;
// Apply contact impulse
b2Vec2 P = lambda * tangent;
vA -= mA * P;
wA -= iA * b2Cross(vcp->rA, P);
vB += mB * P;
wB += iB * b2Cross(vcp->rB, P);
}
// Solve normal constraints
if (vc->pointCount == 1)
{
b2VelocityConstraintPoint* vcp = vc->points + 0;
// Relative velocity at contact
b2Vec2 dv = vB + b2Cross(wB, vcp->rB) - vA - b2Cross(wA, vcp->rA);
// Compute normal impulse
float32 vn = b2Dot(dv, normal);
float32 lambda = -vcp->normalMass * (vn - vcp->velocityBias);
// b2Clamp the accumulated impulse
float32 newImpulse = b2Max(vcp->normalImpulse + lambda, 0.0f);
lambda = newImpulse - vcp->normalImpulse;
vcp->normalImpulse = newImpulse;
// Apply contact impulse
b2Vec2 P = lambda * normal;
vA -= mA * P;
wA -= iA * b2Cross(vcp->rA, P);
vB += mB * P;
wB += iB * b2Cross(vcp->rB, P);
}
else
{
// Block solver developed in collaboration with Dirk Gregorius (back in 01/07 on Box2D_Lite).
// Build the mini LCP for this contact patch
//
// vn = A * x + b, vn >= 0, , vn >= 0, x >= 0 and vn_i * x_i = 0 with i = 1..2
//
// A = J * W * JT and J = ( -n, -r1 x n, n, r2 x n )
// b = vn0 - velocityBias
//
// The system is solved using the "Total enumeration method" (s. Murty). The complementary constraint vn_i * x_i
// implies that we must have in any solution either vn_i = 0 or x_i = 0. So for the 2D contact problem the cases
// vn1 = 0 and vn2 = 0, x1 = 0 and x2 = 0, x1 = 0 and vn2 = 0, x2 = 0 and vn1 = 0 need to be tested. The first valid
// solution that satisfies the problem is chosen.
//
// In order to account of the accumulated impulse 'a' (because of the iterative nature of the solver which only requires
// that the accumulated impulse is clamped and not the incremental impulse) we change the impulse variable (x_i).
//
// Substitute:
//
// x = a + d
//
// a := old total impulse
// x := new total impulse
// d := incremental impulse
//
// For the current iteration we extend the formula for the incremental impulse
// to compute the new total impulse:
//
// vn = A * d + b
// = A * (x - a) + b
// = A * x + b - A * a
// = A * x + b'
// b' = b - A * a;
b2VelocityConstraintPoint* cp1 = vc->points + 0;
b2VelocityConstraintPoint* cp2 = vc->points + 1;
b2Vec2 a(cp1->normalImpulse, cp2->normalImpulse);
b2Assert(a.x >= 0.0f && a.y >= 0.0f);
// Relative velocity at contact
b2Vec2 dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
b2Vec2 dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
float32 vn1 = b2Dot(dv1, normal);
float32 vn2 = b2Dot(dv2, normal);
b2Vec2 b;
b.x = vn1 - cp1->velocityBias;
b.y = vn2 - cp2->velocityBias;
// Compute b'
b -= b2Mul(vc->K, a);
const float32 k_errorTol = 1e-3f;
B2_NOT_USED(k_errorTol);
for (;;)
{
//
// Case 1: vn = 0
//
// 0 = A * x + b'
//
// Solve for x:
//
// x = - inv(A) * b'
//
b2Vec2 x = - b2Mul(vc->normalMass, b);
if (x.x >= 0.0f && x.y >= 0.0f)
{
// Get the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 2: vn1 = 0 and x2 = 0
//
// 0 = a11 * x1 + a12 * 0 + b1'
// vn2 = a21 * x1 + a22 * 0 + b2'
//
x.x = - cp1->normalMass * b.x;
x.y = 0.0f;
vn1 = 0.0f;
vn2 = vc->K.ex.y * x.x + b.y;
if (x.x >= 0.0f && vn2 >= 0.0f)
{
// Get the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv1 = vB + b2Cross(wB, cp1->rB) - vA - b2Cross(wA, cp1->rA);
// Compute normal velocity
vn1 = b2Dot(dv1, normal);
b2Assert(b2Abs(vn1 - cp1->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 3: vn2 = 0 and x1 = 0
//
// vn1 = a11 * 0 + a12 * x2 + b1'
// 0 = a21 * 0 + a22 * x2 + b2'
//
x.x = 0.0f;
x.y = - cp2->normalMass * b.y;
vn1 = vc->K.ey.x * x.y + b.x;
vn2 = 0.0f;
if (x.y >= 0.0f && vn1 >= 0.0f)
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
#if B2_DEBUG_SOLVER == 1
// Postconditions
dv2 = vB + b2Cross(wB, cp2->rB) - vA - b2Cross(wA, cp2->rA);
// Compute normal velocity
vn2 = b2Dot(dv2, normal);
b2Assert(b2Abs(vn2 - cp2->velocityBias) < k_errorTol);
#endif
break;
}
//
// Case 4: x1 = 0 and x2 = 0
//
// vn1 = b1
// vn2 = b2;
x.x = 0.0f;
x.y = 0.0f;
vn1 = b.x;
vn2 = b.y;
if (vn1 >= 0.0f && vn2 >= 0.0f )
{
// Resubstitute for the incremental impulse
b2Vec2 d = x - a;
// Apply incremental impulse
b2Vec2 P1 = d.x * normal;
b2Vec2 P2 = d.y * normal;
vA -= mA * (P1 + P2);
wA -= iA * (b2Cross(cp1->rA, P1) + b2Cross(cp2->rA, P2));
vB += mB * (P1 + P2);
wB += iB * (b2Cross(cp1->rB, P1) + b2Cross(cp2->rB, P2));
// Accumulate
cp1->normalImpulse = x.x;
cp2->normalImpulse = x.y;
break;
}
// No solution, give up. This is hit sometimes, but it doesn't seem to matter.
break;
}
}
m_velocities[indexA].v = vA;
m_velocities[indexA].w = wA;
m_velocities[indexB].v = vB;
m_velocities[indexB].w = wB;
}
}
void b2ContactSolver::StoreImpulses()
{
for (int32 i = 0; i < m_count; ++i)
{
b2ContactVelocityConstraint* vc = m_velocityConstraints + i;
b2Manifold* manifold = m_contacts[vc->contactIndex]->GetManifold();
for (int32 j = 0; j < vc->pointCount; ++j)
{
manifold->points[j].normalImpulse = vc->points[j].normalImpulse;
manifold->points[j].tangentImpulse = vc->points[j].tangentImpulse;
}
}
}
struct b2PositionSolverManifold
{
void Initialize(b2ContactPositionConstraint* pc, const b2Transform& xfA, const b2Transform& xfB, int32 index)
{
b2Assert(pc->pointCount > 0);
switch (pc->type)
{
case b2Manifold::e_circles:
{
b2Vec2 pointA = b2Mul(xfA, pc->localPoint);
b2Vec2 pointB = b2Mul(xfB, pc->localPoints[0]);
normal = pointB - pointA;
normal.Normalize();
point = 0.5f * (pointA + pointB);
separation = b2Dot(pointB - pointA, normal) - pc->radiusA - pc->radiusB;
}
break;
case b2Manifold::e_faceA:
{
normal = b2Mul(xfA.q, pc->localNormal);
b2Vec2 planePoint = b2Mul(xfA, pc->localPoint);
b2Vec2 clipPoint = b2Mul(xfB, pc->localPoints[index]);
separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
point = clipPoint;
}
break;
case b2Manifold::e_faceB:
{
normal = b2Mul(xfB.q, pc->localNormal);
b2Vec2 planePoint = b2Mul(xfB, pc->localPoint);
b2Vec2 clipPoint = b2Mul(xfA, pc->localPoints[index]);
separation = b2Dot(clipPoint - planePoint, normal) - pc->radiusA - pc->radiusB;
point = clipPoint;
// Ensure normal points from A to B
normal = -normal;
}
break;
}
}
b2Vec2 normal;
b2Vec2 point;
float32 separation;
};
// Sequential solver.
bool b2ContactSolver::SolvePositionConstraints()
{
float32 minSeparation = 0.0f;
for (int32 i = 0; i < m_count; ++i)
{
b2ContactPositionConstraint* pc = m_positionConstraints + i;
int32 indexA = pc->indexA;
int32 indexB = pc->indexB;
b2Vec2 localCenterA = pc->localCenterA;
float32 mA = pc->invMassA;
float32 iA = pc->invIA;
b2Vec2 localCenterB = pc->localCenterB;
float32 mB = pc->invMassB;
float32 iB = pc->invIB;
int32 pointCount = pc->pointCount;
b2Vec2 cA = m_positions[indexA].c;
float32 aA = m_positions[indexA].a;
b2Vec2 cB = m_positions[indexB].c;
float32 aB = m_positions[indexB].a;
// Solve normal constraints
for (int32 j = 0; j < pointCount; ++j)
{
b2Transform xfA, xfB;
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - b2Mul(xfA.q, localCenterA);
xfB.p = cB - b2Mul(xfB.q, localCenterB);
b2PositionSolverManifold psm;
psm.Initialize(pc, xfA, xfB, j);
b2Vec2 normal = psm.normal;
b2Vec2 point = psm.point;
float32 separation = psm.separation;
b2Vec2 rA = point - cA;
b2Vec2 rB = point - cB;
// Track max constraint error.
minSeparation = b2Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float32 C = b2Clamp(b2_baumgarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float32 rnA = b2Cross(rA, normal);
float32 rnB = b2Cross(rB, normal);
float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float32 impulse = K > 0.0f ? - C / K : 0.0f;
b2Vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * b2Cross(rA, P);
cB += mB * P;
aB += iB * b2Cross(rB, P);
}
m_positions[indexA].c = cA;
m_positions[indexA].a = aA;
m_positions[indexB].c = cB;
m_positions[indexB].a = aB;
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return minSeparation >= -3.0f * b2_linearSlop;
}
// Sequential position solver for position constraints.
bool b2ContactSolver::SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB)
{
float32 minSeparation = 0.0f;
for (int32 i = 0; i < m_count; ++i)
{
b2ContactPositionConstraint* pc = m_positionConstraints + i;
int32 indexA = pc->indexA;
int32 indexB = pc->indexB;
b2Vec2 localCenterA = pc->localCenterA;
b2Vec2 localCenterB = pc->localCenterB;
int32 pointCount = pc->pointCount;
float32 mA = 0.0f;
float32 iA = 0.0f;
if (indexA == toiIndexA || indexA == toiIndexB)
{
mA = pc->invMassA;
iA = pc->invIA;
}
float32 mB = 0.0f;
float32 iB = 0.;
if (indexB == toiIndexA || indexB == toiIndexB)
{
mB = pc->invMassB;
iB = pc->invIB;
}
b2Vec2 cA = m_positions[indexA].c;
float32 aA = m_positions[indexA].a;
b2Vec2 cB = m_positions[indexB].c;
float32 aB = m_positions[indexB].a;
// Solve normal constraints
for (int32 j = 0; j < pointCount; ++j)
{
b2Transform xfA, xfB;
xfA.q.Set(aA);
xfB.q.Set(aB);
xfA.p = cA - b2Mul(xfA.q, localCenterA);
xfB.p = cB - b2Mul(xfB.q, localCenterB);
b2PositionSolverManifold psm;
psm.Initialize(pc, xfA, xfB, j);
b2Vec2 normal = psm.normal;
b2Vec2 point = psm.point;
float32 separation = psm.separation;
b2Vec2 rA = point - cA;
b2Vec2 rB = point - cB;
// Track max constraint error.
minSeparation = b2Min(minSeparation, separation);
// Prevent large corrections and allow slop.
float32 C = b2Clamp(b2_toiBaugarte * (separation + b2_linearSlop), -b2_maxLinearCorrection, 0.0f);
// Compute the effective mass.
float32 rnA = b2Cross(rA, normal);
float32 rnB = b2Cross(rB, normal);
float32 K = mA + mB + iA * rnA * rnA + iB * rnB * rnB;
// Compute normal impulse
float32 impulse = K > 0.0f ? - C / K : 0.0f;
b2Vec2 P = impulse * normal;
cA -= mA * P;
aA -= iA * b2Cross(rA, P);
cB += mB * P;
aB += iB * b2Cross(rB, P);
}
m_positions[indexA].c = cA;
m_positions[indexA].a = aA;
m_positions[indexB].c = cB;
m_positions[indexB].a = aB;
}
// We can't expect minSpeparation >= -b2_linearSlop because we don't
// push the separation above -b2_linearSlop.
return minSeparation >= -1.5f * b2_linearSlop;
}
@@ -0,0 +1,95 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_CONTACT_SOLVER_H
#define B2_CONTACT_SOLVER_H
#include <Box2D/Common/b2Math.h>
#include <Box2D/Collision/b2Collision.h>
#include <Box2D/Dynamics/b2TimeStep.h>
class b2Contact;
class b2Body;
class b2StackAllocator;
struct b2ContactPositionConstraint;
struct b2VelocityConstraintPoint
{
b2Vec2 rA;
b2Vec2 rB;
float32 normalImpulse;
float32 tangentImpulse;
float32 normalMass;
float32 tangentMass;
float32 velocityBias;
};
struct b2ContactVelocityConstraint
{
b2VelocityConstraintPoint points[b2_maxManifoldPoints];
b2Vec2 normal;
b2Mat22 normalMass;
b2Mat22 K;
int32 indexA;
int32 indexB;
float32 invMassA, invMassB;
float32 invIA, invIB;
float32 friction;
float32 restitution;
float32 tangentSpeed;
int32 pointCount;
int32 contactIndex;
};
struct b2ContactSolverDef
{
b2TimeStep step;
b2Contact** contacts;
int32 count;
b2Position* positions;
b2Velocity* velocities;
b2StackAllocator* allocator;
};
class b2ContactSolver
{
public:
b2ContactSolver(b2ContactSolverDef* def);
~b2ContactSolver();
void InitializeVelocityConstraints();
void WarmStart();
void SolveVelocityConstraints();
void StoreImpulses();
bool SolvePositionConstraints();
bool SolveTOIPositionConstraints(int32 toiIndexA, int32 toiIndexB);
b2TimeStep m_step;
b2Position* m_positions;
b2Velocity* m_velocities;
b2StackAllocator* m_allocator;
b2ContactPositionConstraint* m_positionConstraints;
b2ContactVelocityConstraint* m_velocityConstraints;
b2Contact** m_contacts;
int m_count;
};
#endif
@@ -0,0 +1,49 @@
/*
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Contacts/b2EdgeAndCircleContact.h>
#include <Box2D/Common/b2BlockAllocator.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <new>
b2Contact* b2EdgeAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
{
void* mem = allocator->Allocate(sizeof(b2EdgeAndCircleContact));
return new (mem) b2EdgeAndCircleContact(fixtureA, fixtureB);
}
void b2EdgeAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
{
((b2EdgeAndCircleContact*)contact)->~b2EdgeAndCircleContact();
allocator->Free(contact, sizeof(b2EdgeAndCircleContact));
}
b2EdgeAndCircleContact::b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
: b2Contact(fixtureA, 0, fixtureB, 0)
{
b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
}
void b2EdgeAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
{
b2CollideEdgeAndCircle( manifold,
(b2EdgeShape*)m_fixtureA->GetShape(), xfA,
(b2CircleShape*)m_fixtureB->GetShape(), xfB);
}
@@ -0,0 +1,39 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_EDGE_AND_CIRCLE_CONTACT_H
#define B2_EDGE_AND_CIRCLE_CONTACT_H
#include <Box2D/Dynamics/Contacts/b2Contact.h>
class b2BlockAllocator;
class b2EdgeAndCircleContact : public b2Contact
{
public:
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
b2EdgeAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
~b2EdgeAndCircleContact() {}
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
};
#endif
@@ -0,0 +1,49 @@
/*
* Copyright (c) 2006-2010 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Contacts/b2EdgeAndPolygonContact.h>
#include <Box2D/Common/b2BlockAllocator.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <new>
b2Contact* b2EdgeAndPolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
{
void* mem = allocator->Allocate(sizeof(b2EdgeAndPolygonContact));
return new (mem) b2EdgeAndPolygonContact(fixtureA, fixtureB);
}
void b2EdgeAndPolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
{
((b2EdgeAndPolygonContact*)contact)->~b2EdgeAndPolygonContact();
allocator->Free(contact, sizeof(b2EdgeAndPolygonContact));
}
b2EdgeAndPolygonContact::b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
: b2Contact(fixtureA, 0, fixtureB, 0)
{
b2Assert(m_fixtureA->GetType() == b2Shape::e_edge);
b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
}
void b2EdgeAndPolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
{
b2CollideEdgeAndPolygon( manifold,
(b2EdgeShape*)m_fixtureA->GetShape(), xfA,
(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
}
@@ -0,0 +1,39 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_EDGE_AND_POLYGON_CONTACT_H
#define B2_EDGE_AND_POLYGON_CONTACT_H
#include <Box2D/Dynamics/Contacts/b2Contact.h>
class b2BlockAllocator;
class b2EdgeAndPolygonContact : public b2Contact
{
public:
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
b2EdgeAndPolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
~b2EdgeAndPolygonContact() {}
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
};
#endif
@@ -0,0 +1,49 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Contacts/b2PolygonAndCircleContact.h>
#include <Box2D/Common/b2BlockAllocator.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <new>
b2Contact* b2PolygonAndCircleContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
{
void* mem = allocator->Allocate(sizeof(b2PolygonAndCircleContact));
return new (mem) b2PolygonAndCircleContact(fixtureA, fixtureB);
}
void b2PolygonAndCircleContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
{
((b2PolygonAndCircleContact*)contact)->~b2PolygonAndCircleContact();
allocator->Free(contact, sizeof(b2PolygonAndCircleContact));
}
b2PolygonAndCircleContact::b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
: b2Contact(fixtureA, 0, fixtureB, 0)
{
b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
b2Assert(m_fixtureB->GetType() == b2Shape::e_circle);
}
void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
{
b2CollidePolygonAndCircle( manifold,
(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
(b2CircleShape*)m_fixtureB->GetShape(), xfB);
}
@@ -0,0 +1,38 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_POLYGON_AND_CIRCLE_CONTACT_H
#define B2_POLYGON_AND_CIRCLE_CONTACT_H
#include <Box2D/Dynamics/Contacts/b2Contact.h>
class b2BlockAllocator;
class b2PolygonAndCircleContact : public b2Contact
{
public:
static b2Contact* Create(b2Fixture* fixtureA, int32 indexA, b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
b2PolygonAndCircleContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
~b2PolygonAndCircleContact() {}
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
};
#endif
@@ -0,0 +1,52 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Contacts/b2PolygonContact.h>
#include <Box2D/Common/b2BlockAllocator.h>
#include <Box2D/Collision/b2TimeOfImpact.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2Fixture.h>
#include <Box2D/Dynamics/b2WorldCallbacks.h>
#include <new>
b2Contact* b2PolygonContact::Create(b2Fixture* fixtureA, int32, b2Fixture* fixtureB, int32, b2BlockAllocator* allocator)
{
void* mem = allocator->Allocate(sizeof(b2PolygonContact));
return new (mem) b2PolygonContact(fixtureA, fixtureB);
}
void b2PolygonContact::Destroy(b2Contact* contact, b2BlockAllocator* allocator)
{
((b2PolygonContact*)contact)->~b2PolygonContact();
allocator->Free(contact, sizeof(b2PolygonContact));
}
b2PolygonContact::b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB)
: b2Contact(fixtureA, 0, fixtureB, 0)
{
b2Assert(m_fixtureA->GetType() == b2Shape::e_polygon);
b2Assert(m_fixtureB->GetType() == b2Shape::e_polygon);
}
void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
{
b2CollidePolygons( manifold,
(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
}
@@ -0,0 +1,39 @@
/*
* Copyright (c) 2006-2009 Erin Catto http://www.box2d.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 B2_POLYGON_CONTACT_H
#define B2_POLYGON_CONTACT_H
#include <Box2D/Dynamics/Contacts/b2Contact.h>
class b2BlockAllocator;
class b2PolygonContact : public b2Contact
{
public:
static b2Contact* Create( b2Fixture* fixtureA, int32 indexA,
b2Fixture* fixtureB, int32 indexB, b2BlockAllocator* allocator);
static void Destroy(b2Contact* contact, b2BlockAllocator* allocator);
b2PolygonContact(b2Fixture* fixtureA, b2Fixture* fixtureB);
~b2PolygonContact() {}
void Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB);
};
#endif
@@ -0,0 +1,260 @@
/*
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Joints/b2DistanceJoint.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2TimeStep.h>
// 1-D constrained system
// m (v2 - v1) = lambda
// v2 + (beta/h) * x1 + gamma * lambda = 0, gamma has units of inverse mass.
// x2 = x1 + h * v2
// 1-D mass-damper-spring system
// m (v2 - v1) + h * d * v2 + h * k *
// C = norm(p2 - p1) - L
// u = (p2 - p1) / norm(p2 - p1)
// Cdot = dot(u, v2 + cross(w2, r2) - v1 - cross(w1, r1))
// J = [-u -cross(r1, u) u cross(r2, u)]
// K = J * invM * JT
// = invMass1 + invI1 * cross(r1, u)^2 + invMass2 + invI2 * cross(r2, u)^2
void b2DistanceJointDef::Initialize(b2Body* b1, b2Body* b2,
const b2Vec2& anchor1, const b2Vec2& anchor2)
{
bodyA = b1;
bodyB = b2;
localAnchorA = bodyA->GetLocalPoint(anchor1);
localAnchorB = bodyB->GetLocalPoint(anchor2);
b2Vec2 d = anchor2 - anchor1;
length = d.Length();
}
b2DistanceJoint::b2DistanceJoint(const b2DistanceJointDef* def)
: b2Joint(def)
{
m_localAnchorA = def->localAnchorA;
m_localAnchorB = def->localAnchorB;
m_length = def->length;
m_frequencyHz = def->frequencyHz;
m_dampingRatio = def->dampingRatio;
m_impulse = 0.0f;
m_gamma = 0.0f;
m_bias = 0.0f;
}
void b2DistanceJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexA = m_bodyA->m_islandIndex;
m_indexB = m_bodyB->m_islandIndex;
m_localCenterA = m_bodyA->m_sweep.localCenter;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassA = m_bodyA->m_invMass;
m_invMassB = m_bodyB->m_invMass;
m_invIA = m_bodyA->m_invI;
m_invIB = m_bodyB->m_invI;
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
m_u = cB + m_rB - cA - m_rA;
// Handle singularity.
float32 length = m_u.Length();
if (length > b2_linearSlop)
{
m_u *= 1.0f / length;
}
else
{
m_u.Set(0.0f, 0.0f);
}
float32 crAu = b2Cross(m_rA, m_u);
float32 crBu = b2Cross(m_rB, m_u);
float32 invMass = m_invMassA + m_invIA * crAu * crAu + m_invMassB + m_invIB * crBu * crBu;
// Compute the effective mass matrix.
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
if (m_frequencyHz > 0.0f)
{
float32 C = length - m_length;
// Frequency
float32 omega = 2.0f * b2_pi * m_frequencyHz;
// Damping coefficient
float32 d = 2.0f * m_mass * m_dampingRatio * omega;
// Spring stiffness
float32 k = m_mass * omega * omega;
// magic formulas
float32 h = data.step.dt;
m_gamma = h * (d + h * k);
m_gamma = m_gamma != 0.0f ? 1.0f / m_gamma : 0.0f;
m_bias = C * h * k * m_gamma;
invMass += m_gamma;
m_mass = invMass != 0.0f ? 1.0f / invMass : 0.0f;
}
else
{
m_gamma = 0.0f;
m_bias = 0.0f;
}
if (data.step.warmStarting)
{
// Scale the impulse to support a variable time step.
m_impulse *= data.step.dtRatio;
b2Vec2 P = m_impulse * m_u;
vA -= m_invMassA * P;
wA -= m_invIA * b2Cross(m_rA, P);
vB += m_invMassB * P;
wB += m_invIB * b2Cross(m_rB, P);
}
else
{
m_impulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
void b2DistanceJoint::SolveVelocityConstraints(const b2SolverData& data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
// Cdot = dot(u, v + cross(w, r))
b2Vec2 vpA = vA + b2Cross(wA, m_rA);
b2Vec2 vpB = vB + b2Cross(wB, m_rB);
float32 Cdot = b2Dot(m_u, vpB - vpA);
float32 impulse = -m_mass * (Cdot + m_bias + m_gamma * m_impulse);
m_impulse += impulse;
b2Vec2 P = impulse * m_u;
vA -= m_invMassA * P;
wA -= m_invIA * b2Cross(m_rA, P);
vB += m_invMassB * P;
wB += m_invIB * b2Cross(m_rB, P);
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
bool b2DistanceJoint::SolvePositionConstraints(const b2SolverData& data)
{
if (m_frequencyHz > 0.0f)
{
// There is no position correction for soft distance constraints.
return true;
}
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Rot qA(aA), qB(aB);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 u = cB + rB - cA - rA;
float32 length = u.Normalize();
float32 C = length - m_length;
C = b2Clamp(C, -b2_maxLinearCorrection, b2_maxLinearCorrection);
float32 impulse = -m_mass * C;
b2Vec2 P = impulse * u;
cA -= m_invMassA * P;
aA -= m_invIA * b2Cross(rA, P);
cB += m_invMassB * P;
aB += m_invIB * b2Cross(rB, P);
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
return b2Abs(C) < b2_linearSlop;
}
b2Vec2 b2DistanceJoint::GetAnchorA() const
{
return m_bodyA->GetWorldPoint(m_localAnchorA);
}
b2Vec2 b2DistanceJoint::GetAnchorB() const
{
return m_bodyB->GetWorldPoint(m_localAnchorB);
}
b2Vec2 b2DistanceJoint::GetReactionForce(float32 inv_dt) const
{
b2Vec2 F = (inv_dt * m_impulse) * m_u;
return F;
}
float32 b2DistanceJoint::GetReactionTorque(float32 inv_dt) const
{
B2_NOT_USED(inv_dt);
return 0.0f;
}
void b2DistanceJoint::Dump()
{
int32 indexA = m_bodyA->m_islandIndex;
int32 indexB = m_bodyB->m_islandIndex;
b2Log(" b2DistanceJointDef jd;\n");
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
b2Log(" jd.length = %.15lef;\n", m_length);
b2Log(" jd.frequencyHz = %.15lef;\n", m_frequencyHz);
b2Log(" jd.dampingRatio = %.15lef;\n", m_dampingRatio);
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
}
@@ -0,0 +1,169 @@
/*
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.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 B2_DISTANCE_JOINT_H
#define B2_DISTANCE_JOINT_H
#include <Box2D/Dynamics/Joints/b2Joint.h>
/// Distance joint definition. This requires defining an
/// anchor point on both bodies and the non-zero length of the
/// distance joint. The definition uses local anchor points
/// so that the initial configuration can violate the constraint
/// slightly. This helps when saving and loading a game.
/// @warning Do not use a zero or short length.
struct b2DistanceJointDef : public b2JointDef
{
b2DistanceJointDef()
{
type = e_distanceJoint;
localAnchorA.Set(0.0f, 0.0f);
localAnchorB.Set(0.0f, 0.0f);
length = 1.0f;
frequencyHz = 0.0f;
dampingRatio = 0.0f;
}
/// Initialize the bodies, anchors, and length using the world
/// anchors.
void Initialize(b2Body* bodyA, b2Body* bodyB,
const b2Vec2& anchorA, const b2Vec2& anchorB);
/// The local anchor point relative to bodyA's origin.
b2Vec2 localAnchorA;
/// The local anchor point relative to bodyB's origin.
b2Vec2 localAnchorB;
/// The natural length between the anchor points.
float32 length;
/// The mass-spring-damper frequency in Hertz. A value of 0
/// disables softness.
float32 frequencyHz;
/// The damping ratio. 0 = no damping, 1 = critical damping.
float32 dampingRatio;
};
/// A distance joint constrains two points on two bodies
/// to remain at a fixed distance from each other. You can view
/// this as a massless, rigid rod.
class b2DistanceJoint : public b2Joint
{
public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
/// Get the reaction force given the inverse time step.
/// Unit is N.
b2Vec2 GetReactionForce(float32 inv_dt) const;
/// Get the reaction torque given the inverse time step.
/// Unit is N*m. This is always zero for a distance joint.
float32 GetReactionTorque(float32 inv_dt) const;
/// The local anchor point relative to bodyA's origin.
const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
/// The local anchor point relative to bodyB's origin.
const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
/// Set/get the natural length.
/// Manipulating the length can lead to non-physical behavior when the frequency is zero.
void SetLength(float32 length);
float32 GetLength() const;
/// Set/get frequency in Hz.
void SetFrequency(float32 hz);
float32 GetFrequency() const;
/// Set/get damping ratio.
void SetDampingRatio(float32 ratio);
float32 GetDampingRatio() const;
/// Dump joint to dmLog
void Dump();
protected:
friend class b2Joint;
b2DistanceJoint(const b2DistanceJointDef* data);
void InitVelocityConstraints(const b2SolverData& data);
void SolveVelocityConstraints(const b2SolverData& data);
bool SolvePositionConstraints(const b2SolverData& data);
float32 m_frequencyHz;
float32 m_dampingRatio;
float32 m_bias;
// Solver shared
b2Vec2 m_localAnchorA;
b2Vec2 m_localAnchorB;
float32 m_gamma;
float32 m_impulse;
float32 m_length;
// Solver temp
int32 m_indexA;
int32 m_indexB;
b2Vec2 m_u;
b2Vec2 m_rA;
b2Vec2 m_rB;
b2Vec2 m_localCenterA;
b2Vec2 m_localCenterB;
float32 m_invMassA;
float32 m_invMassB;
float32 m_invIA;
float32 m_invIB;
float32 m_mass;
};
inline void b2DistanceJoint::SetLength(float32 length)
{
m_length = length;
}
inline float32 b2DistanceJoint::GetLength() const
{
return m_length;
}
inline void b2DistanceJoint::SetFrequency(float32 hz)
{
m_frequencyHz = hz;
}
inline float32 b2DistanceJoint::GetFrequency() const
{
return m_frequencyHz;
}
inline void b2DistanceJoint::SetDampingRatio(float32 ratio)
{
m_dampingRatio = ratio;
}
inline float32 b2DistanceJoint::GetDampingRatio() const
{
return m_dampingRatio;
}
#endif
@@ -0,0 +1,251 @@
/*
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Joints/b2FrictionJoint.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2TimeStep.h>
// Point-to-point constraint
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
void b2FrictionJointDef::Initialize(b2Body* bA, b2Body* bB, const b2Vec2& anchor)
{
bodyA = bA;
bodyB = bB;
localAnchorA = bodyA->GetLocalPoint(anchor);
localAnchorB = bodyB->GetLocalPoint(anchor);
}
b2FrictionJoint::b2FrictionJoint(const b2FrictionJointDef* def)
: b2Joint(def)
{
m_localAnchorA = def->localAnchorA;
m_localAnchorB = def->localAnchorB;
m_linearImpulse.SetZero();
m_angularImpulse = 0.0f;
m_maxForce = def->maxForce;
m_maxTorque = def->maxTorque;
}
void b2FrictionJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexA = m_bodyA->m_islandIndex;
m_indexB = m_bodyB->m_islandIndex;
m_localCenterA = m_bodyA->m_sweep.localCenter;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassA = m_bodyA->m_invMass;
m_invMassB = m_bodyB->m_invMass;
m_invIA = m_bodyA->m_invI;
m_invIB = m_bodyB->m_invI;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
// Compute the effective mass matrix.
m_rA = b2Mul(qA, m_localAnchorA - m_localCenterA);
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
b2Mat22 K;
K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
m_linearMass = K.GetInverse();
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse *= data.step.dtRatio;
m_angularImpulse *= data.step.dtRatio;
b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
vA -= mA * P;
wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
vB += mB * P;
wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
void b2FrictionJoint::SolveVelocityConstraints(const b2SolverData& data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
float32 h = data.step.dt;
// Solve angular friction
{
float32 Cdot = wB - wA;
float32 impulse = -m_angularMass * Cdot;
float32 oldImpulse = m_angularImpulse;
float32 maxImpulse = h * m_maxTorque;
m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA);
b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
b2Vec2 oldImpulse = m_linearImpulse;
m_linearImpulse += impulse;
float32 maxImpulse = h * m_maxForce;
if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_linearImpulse.Normalize();
m_linearImpulse *= maxImpulse;
}
impulse = m_linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * b2Cross(m_rA, impulse);
vB += mB * impulse;
wB += iB * b2Cross(m_rB, impulse);
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
bool b2FrictionJoint::SolvePositionConstraints(const b2SolverData& data)
{
B2_NOT_USED(data);
return true;
}
b2Vec2 b2FrictionJoint::GetAnchorA() const
{
return m_bodyA->GetWorldPoint(m_localAnchorA);
}
b2Vec2 b2FrictionJoint::GetAnchorB() const
{
return m_bodyB->GetWorldPoint(m_localAnchorB);
}
b2Vec2 b2FrictionJoint::GetReactionForce(float32 inv_dt) const
{
return inv_dt * m_linearImpulse;
}
float32 b2FrictionJoint::GetReactionTorque(float32 inv_dt) const
{
return inv_dt * m_angularImpulse;
}
void b2FrictionJoint::SetMaxForce(float32 force)
{
b2Assert(b2IsValid(force) && force >= 0.0f);
m_maxForce = force;
}
float32 b2FrictionJoint::GetMaxForce() const
{
return m_maxForce;
}
void b2FrictionJoint::SetMaxTorque(float32 torque)
{
b2Assert(b2IsValid(torque) && torque >= 0.0f);
m_maxTorque = torque;
}
float32 b2FrictionJoint::GetMaxTorque() const
{
return m_maxTorque;
}
void b2FrictionJoint::Dump()
{
int32 indexA = m_bodyA->m_islandIndex;
int32 indexB = m_bodyB->m_islandIndex;
b2Log(" b2FrictionJointDef jd;\n");
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
b2Log(" jd.localAnchorA.Set(%.15lef, %.15lef);\n", m_localAnchorA.x, m_localAnchorA.y);
b2Log(" jd.localAnchorB.Set(%.15lef, %.15lef);\n", m_localAnchorB.x, m_localAnchorB.y);
b2Log(" jd.maxForce = %.15lef;\n", m_maxForce);
b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque);
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
}
@@ -0,0 +1,119 @@
/*
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.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 B2_FRICTION_JOINT_H
#define B2_FRICTION_JOINT_H
#include <Box2D/Dynamics/Joints/b2Joint.h>
/// Friction joint definition.
struct b2FrictionJointDef : public b2JointDef
{
b2FrictionJointDef()
{
type = e_frictionJoint;
localAnchorA.SetZero();
localAnchorB.SetZero();
maxForce = 0.0f;
maxTorque = 0.0f;
}
/// Initialize the bodies, anchors, axis, and reference angle using the world
/// anchor and world axis.
void Initialize(b2Body* bodyA, b2Body* bodyB, const b2Vec2& anchor);
/// The local anchor point relative to bodyA's origin.
b2Vec2 localAnchorA;
/// The local anchor point relative to bodyB's origin.
b2Vec2 localAnchorB;
/// The maximum friction force in N.
float32 maxForce;
/// The maximum friction torque in N-m.
float32 maxTorque;
};
/// Friction joint. This is used for top-down friction.
/// It provides 2D translational friction and angular friction.
class b2FrictionJoint : public b2Joint
{
public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
b2Vec2 GetReactionForce(float32 inv_dt) const;
float32 GetReactionTorque(float32 inv_dt) const;
/// The local anchor point relative to bodyA's origin.
const b2Vec2& GetLocalAnchorA() const { return m_localAnchorA; }
/// The local anchor point relative to bodyB's origin.
const b2Vec2& GetLocalAnchorB() const { return m_localAnchorB; }
/// Set the maximum friction force in N.
void SetMaxForce(float32 force);
/// Get the maximum friction force in N.
float32 GetMaxForce() const;
/// Set the maximum friction torque in N*m.
void SetMaxTorque(float32 torque);
/// Get the maximum friction torque in N*m.
float32 GetMaxTorque() const;
/// Dump joint to dmLog
void Dump();
protected:
friend class b2Joint;
b2FrictionJoint(const b2FrictionJointDef* def);
void InitVelocityConstraints(const b2SolverData& data);
void SolveVelocityConstraints(const b2SolverData& data);
bool SolvePositionConstraints(const b2SolverData& data);
b2Vec2 m_localAnchorA;
b2Vec2 m_localAnchorB;
// Solver shared
b2Vec2 m_linearImpulse;
float32 m_angularImpulse;
float32 m_maxForce;
float32 m_maxTorque;
// Solver temp
int32 m_indexA;
int32 m_indexB;
b2Vec2 m_rA;
b2Vec2 m_rB;
b2Vec2 m_localCenterA;
b2Vec2 m_localCenterB;
float32 m_invMassA;
float32 m_invMassB;
float32 m_invIA;
float32 m_invIB;
b2Mat22 m_linearMass;
float32 m_angularMass;
};
#endif
@@ -0,0 +1,419 @@
/*
* Copyright (c) 2007-2011 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Joints/b2GearJoint.h>
#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2TimeStep.h>
// Gear Joint:
// C0 = (coordinate1 + ratio * coordinate2)_initial
// C = (coordinate1 + ratio * coordinate2) - C0 = 0
// J = [J1 ratio * J2]
// K = J * invM * JT
// = J1 * invM1 * J1T + ratio * ratio * J2 * invM2 * J2T
//
// Revolute:
// coordinate = rotation
// Cdot = angularVelocity
// J = [0 0 1]
// K = J * invM * JT = invI
//
// Prismatic:
// coordinate = dot(p - pg, ug)
// Cdot = dot(v + cross(w, r), ug)
// J = [ug cross(r, ug)]
// K = J * invM * JT = invMass + invI * cross(r, ug)^2
b2GearJoint::b2GearJoint(const b2GearJointDef* def)
: b2Joint(def)
{
m_joint1 = def->joint1;
m_joint2 = def->joint2;
m_typeA = m_joint1->GetType();
m_typeB = m_joint2->GetType();
b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
float32 coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
m_bodyC = m_joint1->GetBodyA();
m_bodyA = m_joint1->GetBodyB();
// Get geometry of joint1
b2Transform xfA = m_bodyA->m_xf;
float32 aA = m_bodyA->m_sweep.a;
b2Transform xfC = m_bodyC->m_xf;
float32 aC = m_bodyC->m_sweep.a;
if (m_typeA == e_revoluteJoint)
{
b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1;
m_localAnchorC = revolute->m_localAnchorA;
m_localAnchorA = revolute->m_localAnchorB;
m_referenceAngleA = revolute->m_referenceAngle;
m_localAxisC.SetZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;
m_localAnchorC = prismatic->m_localAnchorA;
m_localAnchorA = prismatic->m_localAnchorB;
m_referenceAngleA = prismatic->m_referenceAngle;
m_localAxisC = prismatic->m_localXAxisA;
b2Vec2 pC = m_localAnchorC;
b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = b2Dot(pA - pC, m_localAxisC);
}
m_bodyD = m_joint2->GetBodyA();
m_bodyB = m_joint2->GetBodyB();
// Get geometry of joint2
b2Transform xfB = m_bodyB->m_xf;
float32 aB = m_bodyB->m_sweep.a;
b2Transform xfD = m_bodyD->m_xf;
float32 aD = m_bodyD->m_sweep.a;
if (m_typeB == e_revoluteJoint)
{
b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2;
m_localAnchorD = revolute->m_localAnchorA;
m_localAnchorB = revolute->m_localAnchorB;
m_referenceAngleB = revolute->m_referenceAngle;
m_localAxisD.SetZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;
m_localAnchorD = prismatic->m_localAnchorA;
m_localAnchorB = prismatic->m_localAnchorB;
m_referenceAngleB = prismatic->m_referenceAngle;
m_localAxisD = prismatic->m_localXAxisA;
b2Vec2 pD = m_localAnchorD;
b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = b2Dot(pB - pD, m_localAxisD);
}
m_ratio = def->ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}
void b2GearJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexA = m_bodyA->m_islandIndex;
m_indexB = m_bodyB->m_islandIndex;
m_indexC = m_bodyC->m_islandIndex;
m_indexD = m_bodyD->m_islandIndex;
m_lcA = m_bodyA->m_sweep.localCenter;
m_lcB = m_bodyB->m_sweep.localCenter;
m_lcC = m_bodyC->m_sweep.localCenter;
m_lcD = m_bodyD->m_sweep.localCenter;
m_mA = m_bodyA->m_invMass;
m_mB = m_bodyB->m_invMass;
m_mC = m_bodyC->m_invMass;
m_mD = m_bodyD->m_invMass;
m_iA = m_bodyA->m_invI;
m_iB = m_bodyB->m_invI;
m_iC = m_bodyC->m_invI;
m_iD = m_bodyD->m_invI;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
float32 aC = data.positions[m_indexC].a;
b2Vec2 vC = data.velocities[m_indexC].v;
float32 wC = data.velocities[m_indexC].w;
float32 aD = data.positions[m_indexD].a;
b2Vec2 vD = data.velocities[m_indexD].v;
float32 wD = data.velocities[m_indexD].w;
b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
m_mass = 0.0f;
if (m_typeA == e_revoluteJoint)
{
m_JvAC.SetZero();
m_JwA = 1.0f;
m_JwC = 1.0f;
m_mass += m_iA + m_iC;
}
else
{
b2Vec2 u = b2Mul(qC, m_localAxisC);
b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
m_JvAC = u;
m_JwC = b2Cross(rC, u);
m_JwA = b2Cross(rA, u);
m_mass += m_mC + m_mA + m_iC * m_JwC * m_JwC + m_iA * m_JwA * m_JwA;
}
if (m_typeB == e_revoluteJoint)
{
m_JvBD.SetZero();
m_JwB = m_ratio;
m_JwD = m_ratio;
m_mass += m_ratio * m_ratio * (m_iB + m_iD);
}
else
{
b2Vec2 u = b2Mul(qD, m_localAxisD);
b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
m_JvBD = m_ratio * u;
m_JwD = m_ratio * b2Cross(rD, u);
m_JwB = m_ratio * b2Cross(rB, u);
m_mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * m_JwD * m_JwD + m_iB * m_JwB * m_JwB;
}
// Compute effective mass.
m_mass = m_mass > 0.0f ? 1.0f / m_mass : 0.0f;
if (data.step.warmStarting)
{
vA += (m_mA * m_impulse) * m_JvAC;
wA += m_iA * m_impulse * m_JwA;
vB += (m_mB * m_impulse) * m_JvBD;
wB += m_iB * m_impulse * m_JwB;
vC -= (m_mC * m_impulse) * m_JvAC;
wC -= m_iC * m_impulse * m_JwC;
vD -= (m_mD * m_impulse) * m_JvBD;
wD -= m_iD * m_impulse * m_JwD;
}
else
{
m_impulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
data.velocities[m_indexC].v = vC;
data.velocities[m_indexC].w = wC;
data.velocities[m_indexD].v = vD;
data.velocities[m_indexD].w = wD;
}
void b2GearJoint::SolveVelocityConstraints(const b2SolverData& data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Vec2 vC = data.velocities[m_indexC].v;
float32 wC = data.velocities[m_indexC].w;
b2Vec2 vD = data.velocities[m_indexD].v;
float32 wD = data.velocities[m_indexD].w;
float32 Cdot = b2Dot(m_JvAC, vA - vC) + b2Dot(m_JvBD, vB - vD);
Cdot += (m_JwA * wA - m_JwC * wC) + (m_JwB * wB - m_JwD * wD);
float32 impulse = -m_mass * Cdot;
m_impulse += impulse;
vA += (m_mA * impulse) * m_JvAC;
wA += m_iA * impulse * m_JwA;
vB += (m_mB * impulse) * m_JvBD;
wB += m_iB * impulse * m_JwB;
vC -= (m_mC * impulse) * m_JvAC;
wC -= m_iC * impulse * m_JwC;
vD -= (m_mD * impulse) * m_JvBD;
wD -= m_iD * impulse * m_JwD;
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
data.velocities[m_indexC].v = vC;
data.velocities[m_indexC].w = wC;
data.velocities[m_indexD].v = vD;
data.velocities[m_indexD].w = wD;
}
bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 cC = data.positions[m_indexC].c;
float32 aC = data.positions[m_indexC].a;
b2Vec2 cD = data.positions[m_indexD].c;
float32 aD = data.positions[m_indexD].a;
b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
float32 linearError = 0.0f;
float32 coordinateA, coordinateB;
b2Vec2 JvAC, JvBD;
float32 JwA, JwB, JwC, JwD;
float32 mass = 0.0f;
if (m_typeA == e_revoluteJoint)
{
JvAC.SetZero();
JwA = 1.0f;
JwC = 1.0f;
mass += m_iA + m_iC;
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2Vec2 u = b2Mul(qC, m_localAxisC);
b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
JvAC = u;
JwC = b2Cross(rC, u);
JwA = b2Cross(rA, u);
mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
b2Vec2 pC = m_localAnchorC - m_lcC;
b2Vec2 pA = b2MulT(qC, rA + (cA - cC));
coordinateA = b2Dot(pA - pC, m_localAxisC);
}
if (m_typeB == e_revoluteJoint)
{
JvBD.SetZero();
JwB = m_ratio;
JwD = m_ratio;
mass += m_ratio * m_ratio * (m_iB + m_iD);
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2Vec2 u = b2Mul(qD, m_localAxisD);
b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
JvBD = m_ratio * u;
JwD = m_ratio * b2Cross(rD, u);
JwB = m_ratio * b2Cross(rB, u);
mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
b2Vec2 pD = m_localAnchorD - m_lcD;
b2Vec2 pB = b2MulT(qD, rB + (cB - cD));
coordinateB = b2Dot(pB - pD, m_localAxisD);
}
float32 C = (coordinateA + m_ratio * coordinateB) - m_constant;
float32 impulse = 0.0f;
if (mass > 0.0f)
{
impulse = -C / mass;
}
cA += m_mA * impulse * JvAC;
aA += m_iA * impulse * JwA;
cB += m_mB * impulse * JvBD;
aB += m_iB * impulse * JwB;
cC -= m_mC * impulse * JvAC;
aC -= m_iC * impulse * JwC;
cD -= m_mD * impulse * JvBD;
aD -= m_iD * impulse * JwD;
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
data.positions[m_indexC].c = cC;
data.positions[m_indexC].a = aC;
data.positions[m_indexD].c = cD;
data.positions[m_indexD].a = aD;
// TODO_ERIN not implemented
return linearError < b2_linearSlop;
}
b2Vec2 b2GearJoint::GetAnchorA() const
{
return m_bodyA->GetWorldPoint(m_localAnchorA);
}
b2Vec2 b2GearJoint::GetAnchorB() const
{
return m_bodyB->GetWorldPoint(m_localAnchorB);
}
b2Vec2 b2GearJoint::GetReactionForce(float32 inv_dt) const
{
b2Vec2 P = m_impulse * m_JvAC;
return inv_dt * P;
}
float32 b2GearJoint::GetReactionTorque(float32 inv_dt) const
{
float32 L = m_impulse * m_JwA;
return inv_dt * L;
}
void b2GearJoint::SetRatio(float32 ratio)
{
b2Assert(b2IsValid(ratio));
m_ratio = ratio;
}
float32 b2GearJoint::GetRatio() const
{
return m_ratio;
}
void b2GearJoint::Dump()
{
int32 indexA = m_bodyA->m_islandIndex;
int32 indexB = m_bodyB->m_islandIndex;
int32 index1 = m_joint1->m_index;
int32 index2 = m_joint2->m_index;
b2Log(" b2GearJointDef jd;\n");
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
b2Log(" jd.joint1 = joints[%d];\n", index1);
b2Log(" jd.joint2 = joints[%d];\n", index2);
b2Log(" jd.ratio = %.15lef;\n", m_ratio);
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
}
+125
View File
@@ -0,0 +1,125 @@
/*
* Copyright (c) 2006-2011 Erin Catto http://www.box2d.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 B2_GEAR_JOINT_H
#define B2_GEAR_JOINT_H
#include <Box2D/Dynamics/Joints/b2Joint.h>
/// Gear joint definition. This definition requires two existing
/// revolute or prismatic joints (any combination will work).
struct b2GearJointDef : public b2JointDef
{
b2GearJointDef()
{
type = e_gearJoint;
joint1 = NULL;
joint2 = NULL;
ratio = 1.0f;
}
/// The first revolute/prismatic joint attached to the gear joint.
b2Joint* joint1;
/// The second revolute/prismatic joint attached to the gear joint.
b2Joint* joint2;
/// The gear ratio.
/// @see b2GearJoint for explanation.
float32 ratio;
};
/// A gear joint is used to connect two joints together. Either joint
/// can be a revolute or prismatic joint. You specify a gear ratio
/// to bind the motions together:
/// coordinate1 + ratio * coordinate2 = constant
/// The ratio can be negative or positive. If one joint is a revolute joint
/// and the other joint is a prismatic joint, then the ratio will have units
/// of length or units of 1/length.
/// @warning You have to manually destroy the gear joint if joint1 or joint2
/// is destroyed.
class b2GearJoint : public b2Joint
{
public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
b2Vec2 GetReactionForce(float32 inv_dt) const;
float32 GetReactionTorque(float32 inv_dt) const;
/// Get the first joint.
b2Joint* GetJoint1() { return m_joint1; }
/// Get the second joint.
b2Joint* GetJoint2() { return m_joint2; }
/// Set/Get the gear ratio.
void SetRatio(float32 ratio);
float32 GetRatio() const;
/// Dump joint to dmLog
void Dump();
protected:
friend class b2Joint;
b2GearJoint(const b2GearJointDef* data);
void InitVelocityConstraints(const b2SolverData& data);
void SolveVelocityConstraints(const b2SolverData& data);
bool SolvePositionConstraints(const b2SolverData& data);
b2Joint* m_joint1;
b2Joint* m_joint2;
b2JointType m_typeA;
b2JointType m_typeB;
// Body A is connected to body C
// Body B is connected to body D
b2Body* m_bodyC;
b2Body* m_bodyD;
// Solver shared
b2Vec2 m_localAnchorA;
b2Vec2 m_localAnchorB;
b2Vec2 m_localAnchorC;
b2Vec2 m_localAnchorD;
b2Vec2 m_localAxisC;
b2Vec2 m_localAxisD;
float32 m_referenceAngleA;
float32 m_referenceAngleB;
float32 m_constant;
float32 m_ratio;
float32 m_impulse;
// Solver temp
int32 m_indexA, m_indexB, m_indexC, m_indexD;
b2Vec2 m_lcA, m_lcB, m_lcC, m_lcD;
float32 m_mA, m_mB, m_mC, m_mD;
float32 m_iA, m_iB, m_iC, m_iD;
b2Vec2 m_JvAC, m_JvBD;
float32 m_JwA, m_JwB, m_JwC, m_JwD;
float32 m_mass;
};
#endif
+211
View File
@@ -0,0 +1,211 @@
/*
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Joints/b2Joint.h>
#include <Box2D/Dynamics/Joints/b2DistanceJoint.h>
#include <Box2D/Dynamics/Joints/b2WheelJoint.h>
#include <Box2D/Dynamics/Joints/b2MouseJoint.h>
#include <Box2D/Dynamics/Joints/b2RevoluteJoint.h>
#include <Box2D/Dynamics/Joints/b2PrismaticJoint.h>
#include <Box2D/Dynamics/Joints/b2PulleyJoint.h>
#include <Box2D/Dynamics/Joints/b2GearJoint.h>
#include <Box2D/Dynamics/Joints/b2WeldJoint.h>
#include <Box2D/Dynamics/Joints/b2FrictionJoint.h>
#include <Box2D/Dynamics/Joints/b2RopeJoint.h>
#include <Box2D/Dynamics/Joints/b2MotorJoint.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2World.h>
#include <Box2D/Common/b2BlockAllocator.h>
#include <new>
b2Joint* b2Joint::Create(const b2JointDef* def, b2BlockAllocator* allocator)
{
b2Joint* joint = NULL;
switch (def->type)
{
case e_distanceJoint:
{
void* mem = allocator->Allocate(sizeof(b2DistanceJoint));
joint = new (mem) b2DistanceJoint(static_cast<const b2DistanceJointDef*>(def));
}
break;
case e_mouseJoint:
{
void* mem = allocator->Allocate(sizeof(b2MouseJoint));
joint = new (mem) b2MouseJoint(static_cast<const b2MouseJointDef*>(def));
}
break;
case e_prismaticJoint:
{
void* mem = allocator->Allocate(sizeof(b2PrismaticJoint));
joint = new (mem) b2PrismaticJoint(static_cast<const b2PrismaticJointDef*>(def));
}
break;
case e_revoluteJoint:
{
void* mem = allocator->Allocate(sizeof(b2RevoluteJoint));
joint = new (mem) b2RevoluteJoint(static_cast<const b2RevoluteJointDef*>(def));
}
break;
case e_pulleyJoint:
{
void* mem = allocator->Allocate(sizeof(b2PulleyJoint));
joint = new (mem) b2PulleyJoint(static_cast<const b2PulleyJointDef*>(def));
}
break;
case e_gearJoint:
{
void* mem = allocator->Allocate(sizeof(b2GearJoint));
joint = new (mem) b2GearJoint(static_cast<const b2GearJointDef*>(def));
}
break;
case e_wheelJoint:
{
void* mem = allocator->Allocate(sizeof(b2WheelJoint));
joint = new (mem) b2WheelJoint(static_cast<const b2WheelJointDef*>(def));
}
break;
case e_weldJoint:
{
void* mem = allocator->Allocate(sizeof(b2WeldJoint));
joint = new (mem) b2WeldJoint(static_cast<const b2WeldJointDef*>(def));
}
break;
case e_frictionJoint:
{
void* mem = allocator->Allocate(sizeof(b2FrictionJoint));
joint = new (mem) b2FrictionJoint(static_cast<const b2FrictionJointDef*>(def));
}
break;
case e_ropeJoint:
{
void* mem = allocator->Allocate(sizeof(b2RopeJoint));
joint = new (mem) b2RopeJoint(static_cast<const b2RopeJointDef*>(def));
}
break;
case e_motorJoint:
{
void* mem = allocator->Allocate(sizeof(b2MotorJoint));
joint = new (mem) b2MotorJoint(static_cast<const b2MotorJointDef*>(def));
}
break;
default:
b2Assert(false);
break;
}
return joint;
}
void b2Joint::Destroy(b2Joint* joint, b2BlockAllocator* allocator)
{
joint->~b2Joint();
switch (joint->m_type)
{
case e_distanceJoint:
allocator->Free(joint, sizeof(b2DistanceJoint));
break;
case e_mouseJoint:
allocator->Free(joint, sizeof(b2MouseJoint));
break;
case e_prismaticJoint:
allocator->Free(joint, sizeof(b2PrismaticJoint));
break;
case e_revoluteJoint:
allocator->Free(joint, sizeof(b2RevoluteJoint));
break;
case e_pulleyJoint:
allocator->Free(joint, sizeof(b2PulleyJoint));
break;
case e_gearJoint:
allocator->Free(joint, sizeof(b2GearJoint));
break;
case e_wheelJoint:
allocator->Free(joint, sizeof(b2WheelJoint));
break;
case e_weldJoint:
allocator->Free(joint, sizeof(b2WeldJoint));
break;
case e_frictionJoint:
allocator->Free(joint, sizeof(b2FrictionJoint));
break;
case e_ropeJoint:
allocator->Free(joint, sizeof(b2RopeJoint));
break;
case e_motorJoint:
allocator->Free(joint, sizeof(b2MotorJoint));
break;
default:
b2Assert(false);
break;
}
}
b2Joint::b2Joint(const b2JointDef* def)
{
b2Assert(def->bodyA != def->bodyB);
m_type = def->type;
m_prev = NULL;
m_next = NULL;
m_bodyA = def->bodyA;
m_bodyB = def->bodyB;
m_index = 0;
m_collideConnected = def->collideConnected;
m_islandFlag = false;
m_userData = def->userData;
m_edgeA.joint = NULL;
m_edgeA.other = NULL;
m_edgeA.prev = NULL;
m_edgeA.next = NULL;
m_edgeB.joint = NULL;
m_edgeB.other = NULL;
m_edgeB.prev = NULL;
m_edgeB.next = NULL;
}
bool b2Joint::IsActive() const
{
return m_bodyA->IsActive() && m_bodyB->IsActive();
}
+226
View File
@@ -0,0 +1,226 @@
/*
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.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 B2_JOINT_H
#define B2_JOINT_H
#include <Box2D/Common/b2Math.h>
class b2Body;
class b2Joint;
struct b2SolverData;
class b2BlockAllocator;
enum b2JointType
{
e_unknownJoint,
e_revoluteJoint,
e_prismaticJoint,
e_distanceJoint,
e_pulleyJoint,
e_mouseJoint,
e_gearJoint,
e_wheelJoint,
e_weldJoint,
e_frictionJoint,
e_ropeJoint,
e_motorJoint
};
enum b2LimitState
{
e_inactiveLimit,
e_atLowerLimit,
e_atUpperLimit,
e_equalLimits
};
struct b2Jacobian
{
b2Vec2 linear;
float32 angularA;
float32 angularB;
};
/// A joint edge is used to connect bodies and joints together
/// in a joint graph where each body is a node and each joint
/// is an edge. A joint edge belongs to a doubly linked list
/// maintained in each attached body. Each joint has two joint
/// nodes, one for each attached body.
struct b2JointEdge
{
b2Body* other; ///< provides quick access to the other body attached.
b2Joint* joint; ///< the joint
b2JointEdge* prev; ///< the previous joint edge in the body's joint list
b2JointEdge* next; ///< the next joint edge in the body's joint list
};
/// Joint definitions are used to construct joints.
struct b2JointDef
{
b2JointDef()
{
type = e_unknownJoint;
userData = NULL;
bodyA = NULL;
bodyB = NULL;
collideConnected = false;
}
/// The joint type is set automatically for concrete joint types.
b2JointType type;
/// Use this to attach application specific data to your joints.
void* userData;
/// The first attached body.
b2Body* bodyA;
/// The second attached body.
b2Body* bodyB;
/// Set this flag to true if the attached bodies should collide.
bool collideConnected;
};
/// The base joint class. Joints are used to constraint two bodies together in
/// various fashions. Some joints also feature limits and motors.
class b2Joint
{
public:
/// Get the type of the concrete joint.
b2JointType GetType() const;
/// Get the first body attached to this joint.
b2Body* GetBodyA();
/// Get the second body attached to this joint.
b2Body* GetBodyB();
/// Get the anchor point on bodyA in world coordinates.
virtual b2Vec2 GetAnchorA() const = 0;
/// Get the anchor point on bodyB in world coordinates.
virtual b2Vec2 GetAnchorB() const = 0;
/// Get the reaction force on bodyB at the joint anchor in Newtons.
virtual b2Vec2 GetReactionForce(float32 inv_dt) const = 0;
/// Get the reaction torque on bodyB in N*m.
virtual float32 GetReactionTorque(float32 inv_dt) const = 0;
/// Get the next joint the world joint list.
b2Joint* GetNext();
const b2Joint* GetNext() const;
/// Get the user data pointer.
void* GetUserData() const;
/// Set the user data pointer.
void SetUserData(void* data);
/// Short-cut function to determine if either body is inactive.
bool IsActive() const;
/// Get collide connected.
/// Note: modifying the collide connect flag won't work correctly because
/// the flag is only checked when fixture AABBs begin to overlap.
bool GetCollideConnected() const;
/// Dump this joint to the log file.
virtual void Dump() { b2Log("// Dump is not supported for this joint type.\n"); }
/// Shift the origin for any points stored in world coordinates.
virtual void ShiftOrigin(const b2Vec2& newOrigin) { B2_NOT_USED(newOrigin); }
protected:
friend class b2World;
friend class b2Body;
friend class b2Island;
friend class b2GearJoint;
static b2Joint* Create(const b2JointDef* def, b2BlockAllocator* allocator);
static void Destroy(b2Joint* joint, b2BlockAllocator* allocator);
b2Joint(const b2JointDef* def);
virtual ~b2Joint() {}
virtual void InitVelocityConstraints(const b2SolverData& data) = 0;
virtual void SolveVelocityConstraints(const b2SolverData& data) = 0;
// This returns true if the position errors are within tolerance.
virtual bool SolvePositionConstraints(const b2SolverData& data) = 0;
b2JointType m_type;
b2Joint* m_prev;
b2Joint* m_next;
b2JointEdge m_edgeA;
b2JointEdge m_edgeB;
b2Body* m_bodyA;
b2Body* m_bodyB;
int32 m_index;
bool m_islandFlag;
bool m_collideConnected;
void* m_userData;
};
inline b2JointType b2Joint::GetType() const
{
return m_type;
}
inline b2Body* b2Joint::GetBodyA()
{
return m_bodyA;
}
inline b2Body* b2Joint::GetBodyB()
{
return m_bodyB;
}
inline b2Joint* b2Joint::GetNext()
{
return m_next;
}
inline const b2Joint* b2Joint::GetNext() const
{
return m_next;
}
inline void* b2Joint::GetUserData() const
{
return m_userData;
}
inline void b2Joint::SetUserData(void* data)
{
m_userData = data;
}
inline bool b2Joint::GetCollideConnected() const
{
return m_collideConnected;
}
#endif
@@ -0,0 +1,304 @@
/*
* Copyright (c) 2006-2012 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Joints/b2MotorJoint.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2TimeStep.h>
// Point-to-point constraint
// Cdot = v2 - v1
// = v2 + cross(w2, r2) - v1 - cross(w1, r1)
// J = [-I -r1_skew I r2_skew ]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
// Angle constraint
// Cdot = w2 - w1
// J = [0 0 -1 0 0 1]
// K = invI1 + invI2
void b2MotorJointDef::Initialize(b2Body* bA, b2Body* bB)
{
bodyA = bA;
bodyB = bB;
b2Vec2 xB = bodyB->GetPosition();
linearOffset = bodyA->GetLocalPoint(xB);
float32 angleA = bodyA->GetAngle();
float32 angleB = bodyB->GetAngle();
angularOffset = angleB - angleA;
}
b2MotorJoint::b2MotorJoint(const b2MotorJointDef* def)
: b2Joint(def)
{
m_linearOffset = def->linearOffset;
m_angularOffset = def->angularOffset;
m_linearImpulse.SetZero();
m_angularImpulse = 0.0f;
m_maxForce = def->maxForce;
m_maxTorque = def->maxTorque;
m_correctionFactor = def->correctionFactor;
}
void b2MotorJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexA = m_bodyA->m_islandIndex;
m_indexB = m_bodyB->m_islandIndex;
m_localCenterA = m_bodyA->m_sweep.localCenter;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassA = m_bodyA->m_invMass;
m_invMassB = m_bodyB->m_invMass;
m_invIA = m_bodyA->m_invI;
m_invIB = m_bodyB->m_invI;
b2Vec2 cA = data.positions[m_indexA].c;
float32 aA = data.positions[m_indexA].a;
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qA(aA), qB(aB);
// Compute the effective mass matrix.
m_rA = b2Mul(qA, -m_localCenterA);
m_rB = b2Mul(qB, -m_localCenterB);
// J = [-I -r1_skew I r2_skew]
// [ 0 -1 0 1]
// r_skew = [-ry; rx]
// Matlab
// K = [ mA+r1y^2*iA+mB+r2y^2*iB, -r1y*iA*r1x-r2y*iB*r2x, -r1y*iA-r2y*iB]
// [ -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB, r1x*iA+r2x*iB]
// [ -r1y*iA-r2y*iB, r1x*iA+r2x*iB, iA+iB]
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
b2Mat22 K;
K.ex.x = mA + mB + iA * m_rA.y * m_rA.y + iB * m_rB.y * m_rB.y;
K.ex.y = -iA * m_rA.x * m_rA.y - iB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = mA + mB + iA * m_rA.x * m_rA.x + iB * m_rB.x * m_rB.x;
m_linearMass = K.GetInverse();
m_angularMass = iA + iB;
if (m_angularMass > 0.0f)
{
m_angularMass = 1.0f / m_angularMass;
}
m_linearError = cB + m_rB - cA - m_rA - b2Mul(qA, m_linearOffset);
m_angularError = aB - aA - m_angularOffset;
if (data.step.warmStarting)
{
// Scale impulses to support a variable time step.
m_linearImpulse *= data.step.dtRatio;
m_angularImpulse *= data.step.dtRatio;
b2Vec2 P(m_linearImpulse.x, m_linearImpulse.y);
vA -= mA * P;
wA -= iA * (b2Cross(m_rA, P) + m_angularImpulse);
vB += mB * P;
wB += iB * (b2Cross(m_rB, P) + m_angularImpulse);
}
else
{
m_linearImpulse.SetZero();
m_angularImpulse = 0.0f;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
void b2MotorJoint::SolveVelocityConstraints(const b2SolverData& data)
{
b2Vec2 vA = data.velocities[m_indexA].v;
float32 wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
float32 mA = m_invMassA, mB = m_invMassB;
float32 iA = m_invIA, iB = m_invIB;
float32 h = data.step.dt;
float32 inv_h = data.step.inv_dt;
// Solve angular friction
{
float32 Cdot = wB - wA + inv_h * m_correctionFactor * m_angularError;
float32 impulse = -m_angularMass * Cdot;
float32 oldImpulse = m_angularImpulse;
float32 maxImpulse = h * m_maxTorque;
m_angularImpulse = b2Clamp(m_angularImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_angularImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve linear friction
{
b2Vec2 Cdot = vB + b2Cross(wB, m_rB) - vA - b2Cross(wA, m_rA) + inv_h * m_correctionFactor * m_linearError;
b2Vec2 impulse = -b2Mul(m_linearMass, Cdot);
b2Vec2 oldImpulse = m_linearImpulse;
m_linearImpulse += impulse;
float32 maxImpulse = h * m_maxForce;
if (m_linearImpulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_linearImpulse.Normalize();
m_linearImpulse *= maxImpulse;
}
impulse = m_linearImpulse - oldImpulse;
vA -= mA * impulse;
wA -= iA * b2Cross(m_rA, impulse);
vB += mB * impulse;
wB += iB * b2Cross(m_rB, impulse);
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
bool b2MotorJoint::SolvePositionConstraints(const b2SolverData& data)
{
B2_NOT_USED(data);
return true;
}
b2Vec2 b2MotorJoint::GetAnchorA() const
{
return m_bodyA->GetPosition();
}
b2Vec2 b2MotorJoint::GetAnchorB() const
{
return m_bodyB->GetPosition();
}
b2Vec2 b2MotorJoint::GetReactionForce(float32 inv_dt) const
{
return inv_dt * m_linearImpulse;
}
float32 b2MotorJoint::GetReactionTorque(float32 inv_dt) const
{
return inv_dt * m_angularImpulse;
}
void b2MotorJoint::SetMaxForce(float32 force)
{
b2Assert(b2IsValid(force) && force >= 0.0f);
m_maxForce = force;
}
float32 b2MotorJoint::GetMaxForce() const
{
return m_maxForce;
}
void b2MotorJoint::SetMaxTorque(float32 torque)
{
b2Assert(b2IsValid(torque) && torque >= 0.0f);
m_maxTorque = torque;
}
float32 b2MotorJoint::GetMaxTorque() const
{
return m_maxTorque;
}
void b2MotorJoint::SetCorrectionFactor(float32 factor)
{
b2Assert(b2IsValid(factor) && 0.0f <= factor && factor <= 1.0f);
m_correctionFactor = factor;
}
float32 b2MotorJoint::GetCorrectionFactor() const
{
return m_correctionFactor;
}
void b2MotorJoint::SetLinearOffset(const b2Vec2& linearOffset)
{
if (linearOffset.x != m_linearOffset.x || linearOffset.y != m_linearOffset.y)
{
m_bodyA->SetAwake(true);
m_bodyB->SetAwake(true);
m_linearOffset = linearOffset;
}
}
const b2Vec2& b2MotorJoint::GetLinearOffset() const
{
return m_linearOffset;
}
void b2MotorJoint::SetAngularOffset(float32 angularOffset)
{
if (angularOffset != m_angularOffset)
{
m_bodyA->SetAwake(true);
m_bodyB->SetAwake(true);
m_angularOffset = angularOffset;
}
}
float32 b2MotorJoint::GetAngularOffset() const
{
return m_angularOffset;
}
void b2MotorJoint::Dump()
{
int32 indexA = m_bodyA->m_islandIndex;
int32 indexB = m_bodyB->m_islandIndex;
b2Log(" b2MotorJointDef jd;\n");
b2Log(" jd.bodyA = bodies[%d];\n", indexA);
b2Log(" jd.bodyB = bodies[%d];\n", indexB);
b2Log(" jd.collideConnected = bool(%d);\n", m_collideConnected);
b2Log(" jd.linearOffset.Set(%.15lef, %.15lef);\n", m_linearOffset.x, m_linearOffset.y);
b2Log(" jd.angularOffset = %.15lef;\n", m_angularOffset);
b2Log(" jd.maxForce = %.15lef;\n", m_maxForce);
b2Log(" jd.maxTorque = %.15lef;\n", m_maxTorque);
b2Log(" jd.correctionFactor = %.15lef;\n", m_correctionFactor);
b2Log(" joints[%d] = m_world->CreateJoint(&jd);\n", m_index);
}
+133
View File
@@ -0,0 +1,133 @@
/*
* Copyright (c) 2006-2012 Erin Catto http://www.box2d.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 B2_MOTOR_JOINT_H
#define B2_MOTOR_JOINT_H
#include <Box2D/Dynamics/Joints/b2Joint.h>
/// Motor joint definition.
struct b2MotorJointDef : public b2JointDef
{
b2MotorJointDef()
{
type = e_motorJoint;
linearOffset.SetZero();
angularOffset = 0.0f;
maxForce = 1.0f;
maxTorque = 1.0f;
correctionFactor = 0.3f;
}
/// Initialize the bodies and offsets using the current transforms.
void Initialize(b2Body* bodyA, b2Body* bodyB);
/// Position of bodyB minus the position of bodyA, in bodyA's frame, in meters.
b2Vec2 linearOffset;
/// The bodyB angle minus bodyA angle in radians.
float32 angularOffset;
/// The maximum motor force in N.
float32 maxForce;
/// The maximum motor torque in N-m.
float32 maxTorque;
/// Position correction factor in the range [0,1].
float32 correctionFactor;
};
/// A motor joint is used to control the relative motion
/// between two bodies. A typical usage is to control the movement
/// of a dynamic body with respect to the ground.
class b2MotorJoint : public b2Joint
{
public:
b2Vec2 GetAnchorA() const;
b2Vec2 GetAnchorB() const;
b2Vec2 GetReactionForce(float32 inv_dt) const;
float32 GetReactionTorque(float32 inv_dt) const;
/// Set/get the target linear offset, in frame A, in meters.
void SetLinearOffset(const b2Vec2& linearOffset);
const b2Vec2& GetLinearOffset() const;
/// Set/get the target angular offset, in radians.
void SetAngularOffset(float32 angularOffset);
float32 GetAngularOffset() const;
/// Set the maximum friction force in N.
void SetMaxForce(float32 force);
/// Get the maximum friction force in N.
float32 GetMaxForce() const;
/// Set the maximum friction torque in N*m.
void SetMaxTorque(float32 torque);
/// Get the maximum friction torque in N*m.
float32 GetMaxTorque() const;
/// Set the position correction factor in the range [0,1].
void SetCorrectionFactor(float32 factor);
/// Get the position correction factor in the range [0,1].
float32 GetCorrectionFactor() const;
/// Dump to b2Log
void Dump();
protected:
friend class b2Joint;
b2MotorJoint(const b2MotorJointDef* def);
void InitVelocityConstraints(const b2SolverData& data);
void SolveVelocityConstraints(const b2SolverData& data);
bool SolvePositionConstraints(const b2SolverData& data);
// Solver shared
b2Vec2 m_linearOffset;
float32 m_angularOffset;
b2Vec2 m_linearImpulse;
float32 m_angularImpulse;
float32 m_maxForce;
float32 m_maxTorque;
float32 m_correctionFactor;
// Solver temp
int32 m_indexA;
int32 m_indexB;
b2Vec2 m_rA;
b2Vec2 m_rB;
b2Vec2 m_localCenterA;
b2Vec2 m_localCenterB;
b2Vec2 m_linearError;
float32 m_angularError;
float32 m_invMassA;
float32 m_invMassB;
float32 m_invIA;
float32 m_invIB;
b2Mat22 m_linearMass;
float32 m_angularMass;
};
#endif
@@ -0,0 +1,222 @@
/*
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.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 <Box2D/Dynamics/Joints/b2MouseJoint.h>
#include <Box2D/Dynamics/b2Body.h>
#include <Box2D/Dynamics/b2TimeStep.h>
// p = attached point, m = mouse point
// C = p - m
// Cdot = v
// = v + cross(w, r)
// J = [I r_skew]
// Identity used:
// w k % (rx i + ry j) = w * (-ry i + rx j)
b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
: b2Joint(def)
{
b2Assert(def->target.IsValid());
b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);
b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
m_targetA = def->target;
m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);
m_maxForce = def->maxForce;
m_impulse.SetZero();
m_frequencyHz = def->frequencyHz;
m_dampingRatio = def->dampingRatio;
m_beta = 0.0f;
m_gamma = 0.0f;
}
void b2MouseJoint::SetTarget(const b2Vec2& target)
{
if (m_bodyB->IsAwake() == false)
{
m_bodyB->SetAwake(true);
}
m_targetA = target;
}
const b2Vec2& b2MouseJoint::GetTarget() const
{
return m_targetA;
}
void b2MouseJoint::SetMaxForce(float32 force)
{
m_maxForce = force;
}
float32 b2MouseJoint::GetMaxForce() const
{
return m_maxForce;
}
void b2MouseJoint::SetFrequency(float32 hz)
{
m_frequencyHz = hz;
}
float32 b2MouseJoint::GetFrequency() const
{
return m_frequencyHz;
}
void b2MouseJoint::SetDampingRatio(float32 ratio)
{
m_dampingRatio = ratio;
}
float32 b2MouseJoint::GetDampingRatio() const
{
return m_dampingRatio;
}
void b2MouseJoint::InitVelocityConstraints(const b2SolverData& data)
{
m_indexB = m_bodyB->m_islandIndex;
m_localCenterB = m_bodyB->m_sweep.localCenter;
m_invMassB = m_bodyB->m_invMass;
m_invIB = m_bodyB->m_invI;
b2Vec2 cB = data.positions[m_indexB].c;
float32 aB = data.positions[m_indexB].a;
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
b2Rot qB(aB);
float32 mass = m_bodyB->GetMass();
// Frequency
float32 omega = 2.0f * b2_pi * m_frequencyHz;
// Damping coefficient
float32 d = 2.0f * mass * m_dampingRatio * omega;
// Spring stiffness
float32 k = mass * (omega * omega);
// magic formulas
// gamma has units of inverse mass.
// beta has units of inverse time.
float32 h = data.step.dt;
b2Assert(d + h * k > b2_epsilon);
m_gamma = h * (d + h * k);
if (m_gamma != 0.0f)
{
m_gamma = 1.0f / m_gamma;
}
m_beta = h * k * m_gamma;
// Compute the effective mass matrix.
m_rB = b2Mul(qB, m_localAnchorB - m_localCenterB);
// K = [(1/m1 + 1/m2) * eye(2) - skew(r1) * invI1 * skew(r1) - skew(r2) * invI2 * skew(r2)]
// = [1/m1+1/m2 0 ] + invI1 * [r1.y*r1.y -r1.x*r1.y] + invI2 * [r1.y*r1.y -r1.x*r1.y]
// [ 0 1/m1+1/m2] [-r1.x*r1.y r1.x*r1.x] [-r1.x*r1.y r1.x*r1.x]
b2Mat22 K;
K.ex.x = m_invMassB + m_invIB * m_rB.y * m_rB.y + m_gamma;
K.ex.y = -m_invIB * m_rB.x * m_rB.y;
K.ey.x = K.ex.y;
K.ey.y = m_invMassB + m_invIB * m_rB.x * m_rB.x + m_gamma;
m_mass = K.GetInverse();
m_C = cB + m_rB - m_targetA;
m_C *= m_beta;
// Cheat with some damping
wB *= 0.98f;
if (data.step.warmStarting)
{
m_impulse *= data.step.dtRatio;
vB += m_invMassB * m_impulse;
wB += m_invIB * b2Cross(m_rB, m_impulse);
}
else
{
m_impulse.SetZero();
}
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
void b2MouseJoint::SolveVelocityConstraints(const b2SolverData& data)
{
b2Vec2 vB = data.velocities[m_indexB].v;
float32 wB = data.velocities[m_indexB].w;
// Cdot = v + cross(w, r)
b2Vec2 Cdot = vB + b2Cross(wB, m_rB);
b2Vec2 impulse = b2Mul(m_mass, -(Cdot + m_C + m_gamma * m_impulse));
b2Vec2 oldImpulse = m_impulse;
m_impulse += impulse;
float32 maxImpulse = data.step.dt * m_maxForce;
if (m_impulse.LengthSquared() > maxImpulse * maxImpulse)
{
m_impulse *= maxImpulse / m_impulse.Length();
}
impulse = m_impulse - oldImpulse;
vB += m_invMassB * impulse;
wB += m_invIB * b2Cross(m_rB, impulse);
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
bool b2MouseJoint::SolvePositionConstraints(const b2SolverData& data)
{
B2_NOT_USED(data);
return true;
}
b2Vec2 b2MouseJoint::GetAnchorA() const
{
return m_targetA;
}
b2Vec2 b2MouseJoint::GetAnchorB() const
{
return m_bodyB->GetWorldPoint(m_localAnchorB);
}
b2Vec2 b2MouseJoint::GetReactionForce(float32 inv_dt) const
{
return inv_dt * m_impulse;
}
float32 b2MouseJoint::GetReactionTorque(float32 inv_dt) const
{
return inv_dt * 0.0f;
}
void b2MouseJoint::ShiftOrigin(const b2Vec2& newOrigin)
{
m_targetA -= newOrigin;
}
+129
View File
@@ -0,0 +1,129 @@
/*
* Copyright (c) 2006-2007 Erin Catto http://www.box2d.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 B2_MOUSE_JOINT_H
#define B2_MOUSE_JOINT_H
#include <Box2D/Dynamics/Joints/b2Joint.h>
/// Mouse joint definition. This requires a world target point,
/// tuning parameters, and the time step.
struct b2MouseJointDef : public b2JointDef
{
b2MouseJointDef()
{
type = e_mouseJoint;
target.Set(0.0f, 0.0f);
maxForce = 0.0f;
frequencyHz = 5.0f;
dampingRatio = 0.7f;
}
/// The initial world target point. This is assumed
/// to coincide with the body anchor initially.
b2Vec2 target;
/// The maximum constraint force that can be exerted
/// to move the candidate body. Usually you will express
/// as some multiple of the weight (multiplier * mass * gravity).
float32 maxForce;
/// The response speed.
float32 frequencyHz;
/// The damping ratio. 0 = no damping, 1 = critical damping.
float32 dampingRatio;
};
/// A mouse joint is used to make a point on a body track a
/// specified world point. This a soft constraint with a maximum
/// force. This allows the constraint to stretch and without
/// applying huge forces.
/// NOTE: this joint is not documented in the manual because it was
/// developed to be used in the testbed. If you want to learn how to
/// use the mouse joint, look at the testbed.
class b2MouseJoint : public b2Joint
{
public:
/// Implements b2Joint.
b2Vec2 GetAnchorA() const;
/// Implements b2Joint.
b2Vec2 GetAnchorB() const;
/// Implements b2Joint.
b2Vec2 GetReactionForce(float32 inv_dt) const;
/// Implements b2Joint.
float32 GetReactionTorque(float32 inv_dt) const;
/// Use this to update the target point.
void SetTarget(const b2Vec2& target);
const b2Vec2& GetTarget() const;
/// Set/get the maximum force in Newtons.
void SetMaxForce(float32 force);
float32 GetMaxForce() const;
/// Set/get the frequency in Hertz.
void SetFrequency(float32 hz);
float32 GetFrequency() const;
/// Set/get the damping ratio (dimensionless).
void SetDampingRatio(float32 ratio);
float32 GetDampingRatio() const;
/// The mouse joint does not support dumping.
void Dump() { b2Log("Mouse joint dumping is not supported.\n"); }
/// Implement b2Joint::ShiftOrigin
void ShiftOrigin(const b2Vec2& newOrigin);
protected:
friend class b2Joint;
b2MouseJoint(const b2MouseJointDef* def);
void InitVelocityConstraints(const b2SolverData& data);
void SolveVelocityConstraints(const b2SolverData& data);
bool SolvePositionConstraints(const b2SolverData& data);
b2Vec2 m_localAnchorB;
b2Vec2 m_targetA;
float32 m_frequencyHz;
float32 m_dampingRatio;
float32 m_beta;
// Solver shared
b2Vec2 m_impulse;
float32 m_maxForce;
float32 m_gamma;
// Solver temp
int32 m_indexA;
int32 m_indexB;
b2Vec2 m_rB;
b2Vec2 m_localCenterB;
float32 m_invMassB;
float32 m_invIB;
b2Mat22 m_mass;
b2Vec2 m_C;
};
#endif

Some files were not shown because too many files have changed in this diff Show More