rollmorad hinzugefügt
This commit is contained in:
@@ -0,0 +1,477 @@
|
||||
/*
|
||||
Copyright (c) 2006 Henry Strickland & Ryan Seto
|
||||
2007-2008 Tobias Weyand (modifications and extensions)
|
||||
|
||||
Permission is hereby granted, free of charge, to any person obtaining a
|
||||
copy of this software and associated documentation files (the "Software"),
|
||||
to deal in the Software without restriction, including without limitation
|
||||
the rights to use, copy, modify, merge, publish, distribute, sublicense,
|
||||
and/or sell copies of the Software, and to permit persons to whom the
|
||||
Software is furnished to do so, subject to the following conditions:
|
||||
|
||||
The above copyright notice and this permission notice shall be included
|
||||
in all copies or substantial portions of the Software.
|
||||
|
||||
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
|
||||
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
|
||||
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
|
||||
THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR
|
||||
OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE,
|
||||
ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR
|
||||
OTHER DEALINGS IN THE SOFTWARE.
|
||||
|
||||
(* http://www.opensource.org/licenses/mit-license.php *)
|
||||
*/
|
||||
|
||||
#ifndef _FIXED_H_
|
||||
#define _FIXED_H_
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#ifdef TARGET_IS_NDS
|
||||
|
||||
#include "nds.h"
|
||||
|
||||
#endif
|
||||
|
||||
#define FIXED_BP 16
|
||||
#define FIXED_MAX ((1<<(32-FIXED_BP-1))-1)
|
||||
#define FIXED_MIN (-(1<<(32-FIXED_BP-1)))
|
||||
#define FIXED_EPSILON (Fixed(0.00007f))
|
||||
|
||||
#define G_1_DIV_PI 20861
|
||||
|
||||
class Fixed {
|
||||
|
||||
private:
|
||||
|
||||
int g; // the guts
|
||||
|
||||
const static int BP= FIXED_BP; // how many low bits are right of Binary Point
|
||||
const static int BP2= BP*2; // how many low bits are right of Binary Point
|
||||
const static int BPhalf= BP/2; // how many low bits are right of Binary Point
|
||||
|
||||
double STEP(); // smallest step we can represent
|
||||
|
||||
// for private construction via guts
|
||||
enum FixedRaw { RAW };
|
||||
Fixed(FixedRaw, int guts);
|
||||
|
||||
public:
|
||||
|
||||
Fixed();
|
||||
Fixed(const Fixed &a);
|
||||
Fixed(float a);
|
||||
Fixed(double a);
|
||||
Fixed(int a);
|
||||
Fixed(long a);
|
||||
|
||||
Fixed& operator =(const Fixed a);
|
||||
Fixed& operator =(float a);
|
||||
Fixed& operator =(double a);
|
||||
Fixed& operator =(int a);
|
||||
Fixed& operator =(long a);
|
||||
|
||||
operator float();
|
||||
operator double();
|
||||
operator int();
|
||||
operator long();
|
||||
operator unsigned short();
|
||||
|
||||
operator float() const;
|
||||
|
||||
Fixed operator +() const;
|
||||
Fixed operator -() const;
|
||||
|
||||
Fixed operator +(const Fixed a) const;
|
||||
Fixed operator -(const Fixed a) const;
|
||||
#if 1
|
||||
// more acurate, using long long
|
||||
Fixed operator *(const Fixed a) const;
|
||||
#else
|
||||
// faster, but with only half as many bits right of binary point
|
||||
Fixed operator *(const Fixed a) const;
|
||||
#endif
|
||||
Fixed operator /(const Fixed a) const;
|
||||
|
||||
Fixed operator *(unsigned short a) const;
|
||||
Fixed operator *(int a) const;
|
||||
|
||||
Fixed operator +(float a) const;
|
||||
Fixed operator -(float a) const;
|
||||
Fixed operator *(float a) const;
|
||||
Fixed operator /(float a) const;
|
||||
|
||||
Fixed operator +(double a) const;
|
||||
Fixed operator -(double a) const;
|
||||
Fixed operator *(double a) const;
|
||||
Fixed operator /(double a) const;
|
||||
|
||||
Fixed operator >>(int a) const;
|
||||
Fixed operator <<(int a) const;
|
||||
|
||||
Fixed& operator +=(Fixed a);
|
||||
Fixed& operator -=(Fixed a);
|
||||
Fixed& operator *=(Fixed a);
|
||||
Fixed& operator /=(Fixed a);
|
||||
|
||||
Fixed& operator +=(int a);
|
||||
Fixed& operator -=(int a);
|
||||
Fixed& operator *=(int a);
|
||||
Fixed& operator /=(int a);
|
||||
|
||||
Fixed& operator +=(long a);
|
||||
Fixed& operator -=(long a);
|
||||
Fixed& operator *=(long a);
|
||||
Fixed& operator /=(long a);
|
||||
|
||||
Fixed& operator +=(float a);
|
||||
Fixed& operator -=(float a);
|
||||
Fixed& operator *=(float a);
|
||||
Fixed& operator /=(float a);
|
||||
|
||||
Fixed& operator +=(double a);
|
||||
Fixed& operator -=(double a);
|
||||
Fixed& operator *=(double a);
|
||||
Fixed& operator /=(double a);
|
||||
|
||||
bool operator ==(const Fixed a) const;
|
||||
bool operator !=(const Fixed a) const;
|
||||
bool operator <=(const Fixed a) const;
|
||||
bool operator >=(const Fixed a) const;
|
||||
bool operator <(const Fixed a) const;
|
||||
bool operator >(const Fixed a) const;
|
||||
|
||||
bool operator ==(float a) const;
|
||||
bool operator !=(float a) const;
|
||||
bool operator <=(float a) const;
|
||||
bool operator >=(float a) const;
|
||||
bool operator <(float a) const;
|
||||
bool operator >(float a) const;
|
||||
|
||||
bool operator ==(double a) const;
|
||||
bool operator !=(double a) const;
|
||||
bool operator <=(double a) const;
|
||||
bool operator >=(double a) const;
|
||||
bool operator <(double a) const;
|
||||
bool operator >(double a) const;
|
||||
|
||||
bool operator >(int a) const;
|
||||
bool operator <(int a) const;
|
||||
bool operator >=(int a) const;
|
||||
bool operator <=(int a) const;
|
||||
|
||||
Fixed abs();
|
||||
Fixed sqrt();
|
||||
#ifdef TARGET_IS_NDS
|
||||
Fixed cosf();
|
||||
Fixed sinf();
|
||||
Fixed tanf();
|
||||
#endif
|
||||
};
|
||||
|
||||
//
|
||||
// Implementation
|
||||
//
|
||||
|
||||
inline double Fixed::STEP() { return 1.0 / (1<<BP); } // smallest step we can represent
|
||||
|
||||
// for private construction via guts
|
||||
inline Fixed::Fixed(FixedRaw, int guts) : g(guts) {}
|
||||
|
||||
inline Fixed::Fixed() : g(0) {}
|
||||
inline Fixed::Fixed(const Fixed &a) : g( a.g ) {}
|
||||
inline Fixed::Fixed(float a) : g( int(a * (float)(1<<BP)) ) {}
|
||||
inline Fixed::Fixed(double a) : g( int(a * (double)(1<<BP) ) ) {}
|
||||
inline Fixed::Fixed(int a) : g( a << BP ) {}
|
||||
inline Fixed::Fixed(long a) : g( a << BP ) {}
|
||||
|
||||
inline Fixed& Fixed::operator =(const Fixed a) { g= a.g; return *this; }
|
||||
inline Fixed& Fixed::operator =(float a) { g= Fixed(a).g; return *this; }
|
||||
inline Fixed& Fixed::operator =(double a) { g= Fixed(a).g; return *this; }
|
||||
inline Fixed& Fixed::operator =(int a) { g= Fixed(a).g; return *this; }
|
||||
inline Fixed& Fixed::operator =(long a) { g= Fixed(a).g; return *this; }
|
||||
|
||||
inline Fixed::operator float() { return g * (float)STEP(); }
|
||||
inline Fixed::operator double() { return g * (double)STEP(); }
|
||||
inline Fixed::operator int() { return g>>BP; }
|
||||
inline Fixed::operator long() { return g>>BP; }
|
||||
//#pragma warning(disable: 4244) //HARDWIRE added pragma to prevent VS2005 compilation error
|
||||
inline Fixed::operator unsigned short() { return g>>BP; }
|
||||
inline Fixed::operator float() const { return g / (float)(1<<BP); }
|
||||
|
||||
inline Fixed Fixed::operator +() const { return Fixed(RAW,g); }
|
||||
inline Fixed Fixed::operator -() const { return Fixed(RAW,-g); }
|
||||
|
||||
inline Fixed Fixed::operator +(const Fixed a) const { return Fixed(RAW, g + a.g); }
|
||||
inline Fixed Fixed::operator -(const Fixed a) const { return Fixed(RAW, g - a.g); }
|
||||
|
||||
#if 1
|
||||
// more acurate, using long long
|
||||
inline Fixed Fixed::operator *(const Fixed a) const { return Fixed(RAW, (int)( ((long long)g * (long long)a.g ) >> BP)); }
|
||||
|
||||
#elif 0
|
||||
|
||||
// check for overflow and figure out where. Must specify -rdynamic in linker
|
||||
#include <execinfo.h>
|
||||
#include <signal.h>
|
||||
#include <exception>
|
||||
|
||||
inline Fixed Fixed::operator *(const Fixed a) const {
|
||||
long long x = ((long long)g * (long long)a.g );
|
||||
if(x > 0x7fffffffffffLL || x < -0x7fffffffffffLL) {
|
||||
printf("overflow");
|
||||
void *array[2];
|
||||
int nSize = backtrace(array, 2);
|
||||
char **symbols = backtrace_symbols(array, nSize);
|
||||
for(int i=0; i<nSize; i++) {
|
||||
printf(" %s", symbols[i]);
|
||||
}
|
||||
printf("\n");
|
||||
}
|
||||
return Fixed(RAW, (int)(x>>BP));
|
||||
}
|
||||
|
||||
#else
|
||||
// faster, but with only half as many bits right of binary point
|
||||
inline Fixed Fixed::operator *(const Fixed a) const { return Fixed(RAW, (g>>BPhalf) * (a.g>>BPhalf) ); }
|
||||
#endif
|
||||
|
||||
|
||||
#ifdef TARGET_IS_NDS
|
||||
// Division using the DS's maths coprocessor
|
||||
inline Fixed Fixed::operator /(const Fixed a) const
|
||||
{
|
||||
//printf("%d %d\n", (long long)g << BP, a.g);
|
||||
return Fixed(RAW, int( div64((long long)g << BP, a.g) ) );
|
||||
}
|
||||
#else
|
||||
inline Fixed Fixed::operator /(const Fixed a) const
|
||||
{
|
||||
return Fixed(RAW, int( (((long long)g << BP2) / (long long)(a.g)) >> BP) );
|
||||
//return Fixed(RAW, int( (((long long)g << BP) / (long long)(a.g)) ) );
|
||||
}
|
||||
#endif
|
||||
|
||||
inline Fixed Fixed::operator *(unsigned short a) const { return operator*(Fixed(a)); }
|
||||
inline Fixed Fixed::operator *(int a) const { return operator*(Fixed(a)); }
|
||||
|
||||
inline Fixed Fixed::operator +(float a) const { return Fixed(RAW, g + Fixed(a).g); }
|
||||
inline Fixed Fixed::operator -(float a) const { return Fixed(RAW, g - Fixed(a).g); }
|
||||
inline Fixed Fixed::operator *(float a) const { return Fixed(RAW, (g>>BPhalf) * (Fixed(a).g>>BPhalf) ); }
|
||||
//inline Fixed Fixed::operator /(float a) const { return Fixed(RAW, int( (((long long)g << BP2) / (long long)(Fixed(a).g)) >> BP) ); }
|
||||
inline Fixed Fixed::operator /(float a) const { return operator/(Fixed(a)); }
|
||||
|
||||
inline Fixed Fixed::operator +(double a) const { return Fixed(RAW, g + Fixed(a).g); }
|
||||
inline Fixed Fixed::operator -(double a) const { return Fixed(RAW, g - Fixed(a).g); }
|
||||
inline Fixed Fixed::operator *(double a) const { return Fixed(RAW, (g>>BPhalf) * (Fixed(a).g>>BPhalf) ); }
|
||||
//inline Fixed Fixed::operator /(double a) const { return Fixed(RAW, int( (((long long)g << BP2) / (long long)(Fixed(a).g)) >> BP) ); }
|
||||
inline Fixed Fixed::operator /(double a) const { return operator/(Fixed(a)); }
|
||||
|
||||
inline Fixed Fixed::operator >>(int a) const { return Fixed(RAW, g >> a); }
|
||||
inline Fixed Fixed::operator <<(int a) const { return Fixed(RAW, g << a); }
|
||||
|
||||
inline Fixed& Fixed::operator +=(Fixed a) { return *this = *this + a; }
|
||||
inline Fixed& Fixed::operator -=(Fixed a) { return *this = *this - a; }
|
||||
inline Fixed& Fixed::operator *=(Fixed a) { return *this = *this * a; }
|
||||
//inline Fixed& Fixed::operator /=(Fixed a) { return *this = *this / a; }
|
||||
inline Fixed& Fixed::operator /=(Fixed a) { return *this = operator/(a); }
|
||||
|
||||
inline Fixed& Fixed::operator +=(int a) { return *this = *this + (Fixed)a; }
|
||||
inline Fixed& Fixed::operator -=(int a) { return *this = *this - (Fixed)a; }
|
||||
inline Fixed& Fixed::operator *=(int a) { return *this = *this * (Fixed)a; }
|
||||
//inline Fixed& Fixed::operator /=(int a) { return *this = *this / (Fixed)a; }
|
||||
inline Fixed& Fixed::operator /=(int a) { return *this = operator/((Fixed)a); }
|
||||
|
||||
inline Fixed& Fixed::operator +=(long a) { return *this = *this + (Fixed)a; }
|
||||
inline Fixed& Fixed::operator -=(long a) { return *this = *this - (Fixed)a; }
|
||||
inline Fixed& Fixed::operator *=(long a) { return *this = *this * (Fixed)a; }
|
||||
//inline Fixed& Fixed::operator /=(long a) { return *this = *this / (Fixed)a; }
|
||||
inline Fixed& Fixed::operator /=(long a) { return *this = operator/((Fixed)a); }
|
||||
|
||||
inline Fixed& Fixed::operator +=(float a) { return *this = *this + a; }
|
||||
inline Fixed& Fixed::operator -=(float a) { return *this = *this - a; }
|
||||
inline Fixed& Fixed::operator *=(float a) { return *this = *this * a; }
|
||||
//inline Fixed& Fixed::operator /=(float a) { return *this = *this / a; }
|
||||
inline Fixed& Fixed::operator /=(float a) { return *this = operator/(a); }
|
||||
|
||||
inline Fixed& Fixed::operator +=(double a) { return *this = *this + a; }
|
||||
inline Fixed& Fixed::operator -=(double a) { return *this = *this - a; }
|
||||
inline Fixed& Fixed::operator *=(double a) { return *this = *this * a; }
|
||||
//inline Fixed& Fixed::operator /=(double a) { return *this = *this / a; }
|
||||
inline Fixed& Fixed::operator /=(double a) { return *this = operator/(a); }
|
||||
|
||||
inline Fixed operator +(int a, const Fixed b) { return Fixed(a)+b; }
|
||||
inline Fixed operator -(int a, const Fixed b) { return Fixed(a)-b; }
|
||||
inline Fixed operator *(int a, const Fixed b) { return Fixed(a)*b; }
|
||||
inline Fixed operator /(int a, const Fixed b) { return Fixed(a)/b; };
|
||||
|
||||
inline Fixed operator +(float a, const Fixed b) { return Fixed(a)+b; }
|
||||
inline Fixed operator -(float a, const Fixed b) { return Fixed(a)-b; }
|
||||
inline Fixed operator *(float a, const Fixed b) { return Fixed(a)*b; }
|
||||
inline Fixed operator /(float a, const Fixed b) { return Fixed(a)/b; }
|
||||
|
||||
inline bool Fixed::operator ==(const Fixed a) const { return g == a.g; }
|
||||
inline bool Fixed::operator !=(const Fixed a) const { return g != a.g; }
|
||||
inline bool Fixed::operator <=(const Fixed a) const { return g <= a.g; }
|
||||
inline bool Fixed::operator >=(const Fixed a) const { return g >= a.g; }
|
||||
inline bool Fixed::operator <(const Fixed a) const { return g < a.g; }
|
||||
inline bool Fixed::operator >(const Fixed a) const { return g > a.g; }
|
||||
|
||||
inline bool Fixed::operator ==(float a) const { return g == Fixed(a).g; }
|
||||
inline bool Fixed::operator !=(float a) const { return g != Fixed(a).g; }
|
||||
inline bool Fixed::operator <=(float a) const { return g <= Fixed(a).g; }
|
||||
inline bool Fixed::operator >=(float a) const { return g >= Fixed(a).g; }
|
||||
inline bool Fixed::operator <(float a) const { return g < Fixed(a).g; }
|
||||
inline bool Fixed::operator >(float a) const { return g > Fixed(a).g; }
|
||||
|
||||
inline bool Fixed::operator ==(double a) const { return g == Fixed(a).g; }
|
||||
inline bool Fixed::operator !=(double a) const { return g != Fixed(a).g; }
|
||||
inline bool Fixed::operator <=(double a) const { return g <= Fixed(a).g; }
|
||||
inline bool Fixed::operator >=(double a) const { return g >= Fixed(a).g; }
|
||||
inline bool Fixed::operator <(double a) const { return g < Fixed(a).g; }
|
||||
inline bool Fixed::operator >(double a) const { return g > Fixed(a).g; }
|
||||
|
||||
inline bool Fixed::operator >(int a) const { return g > Fixed(a).g; }
|
||||
inline bool Fixed::operator <(int a) const { return g < Fixed(a).g; }
|
||||
inline bool Fixed::operator >=(int a) const{ return g >= Fixed(a).g; };
|
||||
inline bool Fixed::operator <=(int a) const{ return g <= Fixed(a).g; };
|
||||
|
||||
inline bool operator ==(float a, const Fixed b) { return Fixed(a) == b; }
|
||||
inline bool operator !=(float a, const Fixed b) { return Fixed(a) != b; }
|
||||
inline bool operator <=(float a, const Fixed b) { return Fixed(a) <= b; }
|
||||
inline bool operator >=(float a, const Fixed b) { return Fixed(a) >= b; }
|
||||
inline bool operator <(float a, const Fixed b) { return Fixed(a) < b; }
|
||||
inline bool operator >(float a, const Fixed b) { return Fixed(a) > b; }
|
||||
|
||||
inline Fixed operator +(double a, const Fixed b) { return Fixed(a)+b; }
|
||||
inline Fixed operator -(double a, const Fixed b) { return Fixed(a)-b; }
|
||||
inline Fixed operator *(double a, const Fixed b) { return Fixed(a)*b; }
|
||||
inline Fixed operator /(double a, const Fixed b) { return Fixed(a)/b; }
|
||||
|
||||
inline bool operator ==(double a, const Fixed b) { return Fixed(a) == b; }
|
||||
inline bool operator !=(double a, const Fixed b) { return Fixed(a) != b; }
|
||||
inline bool operator <=(double a, const Fixed b) { return Fixed(a) <= b; }
|
||||
inline bool operator >=(double a, const Fixed b) { return Fixed(a) >= b; }
|
||||
inline bool operator <(double a, const Fixed b) { return Fixed(a) < b; }
|
||||
inline bool operator >(double a, const Fixed b) { return Fixed(a) > b; }
|
||||
|
||||
inline bool operator ==(int a, const Fixed b) { return Fixed(a) == b; }
|
||||
inline bool operator !=(int a, const Fixed b) { return Fixed(a) != b; }
|
||||
inline bool operator <=(int a, const Fixed b) { return Fixed(a) <= b; }
|
||||
inline bool operator >=(int a, const Fixed b) { return Fixed(a) >= b; }
|
||||
inline bool operator <(int a, const Fixed b) { return Fixed(a) < b; }
|
||||
inline bool operator >(int a, const Fixed b) { return Fixed(a) > b; }
|
||||
|
||||
inline int& operator +=(int& a, const Fixed b) { a = (Fixed)a + b; return a; }
|
||||
inline int& operator -=(int& a, const Fixed b) { a = (Fixed)a - b; return a; }
|
||||
inline int& operator *=(int& a, const Fixed b) { a = (Fixed)a * b; return a; }
|
||||
inline int& operator /=(int& a, const Fixed b) { a = (Fixed)a / b; return a; }
|
||||
|
||||
inline long& operator +=(long& a, const Fixed b) { a = (Fixed)a + b; return a; }
|
||||
inline long& operator -=(long& a, const Fixed b) { a = (Fixed)a - b; return a; }
|
||||
inline long& operator *=(long& a, const Fixed b) { a = (Fixed)a * b; return a; }
|
||||
inline long& operator /=(long& a, const Fixed b) { a = (Fixed)a / b; return a; }
|
||||
|
||||
inline float& operator +=(float& a, const Fixed b) { a = a + b; return a; }
|
||||
inline float& operator -=(float& a, const Fixed b) { a = a - b; return a; }
|
||||
inline float& operator *=(float& a, const Fixed b) { a = a * b; return a; }
|
||||
inline float& operator /=(float& a, const Fixed b) { a = a / b; return a; }
|
||||
|
||||
inline double& operator +=(double& a, const Fixed b) { a = a + b; return a; }
|
||||
inline double& operator -=(double& a, const Fixed b) { a = a - b; return a; }
|
||||
inline double& operator *=(double& a, const Fixed b) { a = a * b; return a; }
|
||||
inline double& operator /=(double& a, const Fixed b) { a = a / b; return a; }
|
||||
|
||||
inline Fixed Fixed::abs() { return (g>0) ? Fixed(RAW, g) : Fixed(RAW, -g); }
|
||||
inline Fixed abs(Fixed f) { return f.abs(); }
|
||||
|
||||
//inline Fixed atan2(Fixed a, Fixed b) { return atan2f((float) a, (float) b); }
|
||||
inline Fixed atan2(Fixed y, Fixed x)
|
||||
{
|
||||
Fixed abs_y = y.abs() + FIXED_EPSILON; // avoid 0/0
|
||||
Fixed r, angle;
|
||||
|
||||
if(x >= 0.0f) {
|
||||
r = (x - abs_y) / (x + abs_y);
|
||||
angle = 3.1415926/4.0;
|
||||
} else {
|
||||
r = (x + abs_y) / (abs_y - x);
|
||||
angle = 3.0*3.1415926/4.0;
|
||||
}
|
||||
angle += Fixed(0.1963) * (r * r * r) - Fixed(0.9817) * r;
|
||||
return (y < 0) ? -angle : angle;
|
||||
}
|
||||
|
||||
#if TARGET_IS_NDS
|
||||
|
||||
static inline long nds_sqrt64(long long a)
|
||||
{
|
||||
SQRT_CR = SQRT_64;
|
||||
while(SQRT_CR & SQRT_BUSY);
|
||||
SQRT_PARAM64 = a;
|
||||
while(SQRT_CR & SQRT_BUSY);
|
||||
|
||||
return SQRT_RESULT32;
|
||||
}
|
||||
|
||||
static inline int32 div6464(int64 num, int64 den)
|
||||
{
|
||||
DIV_CR = DIV_64_64;
|
||||
while(DIV_CR & DIV_BUSY);
|
||||
DIV_NUMERATOR64 = num;
|
||||
DIV_DENOMINATOR64 = den;
|
||||
while(DIV_CR & DIV_BUSY);
|
||||
|
||||
return (DIV_RESULT32);
|
||||
}
|
||||
|
||||
inline Fixed Fixed::sqrt()
|
||||
{
|
||||
return Fixed(RAW, nds_sqrt64(((long long)(g))<<BP));
|
||||
}
|
||||
#else
|
||||
inline Fixed Fixed::sqrt()
|
||||
{
|
||||
long long m, root = 0, left = (long long)g<<FIXED_BP;
|
||||
for ( m = (long long)1<<( (sizeof(long long)<<3) - 2); m; m >>= 2 )
|
||||
{
|
||||
if ( ( left & -m ) > root )
|
||||
left -= ( root += m ), root += m;
|
||||
root >>= 1;
|
||||
}
|
||||
return Fixed(RAW, root);
|
||||
}
|
||||
#endif
|
||||
|
||||
inline Fixed sqrt(Fixed a) { return a.sqrt(); }
|
||||
inline Fixed sqrtf(Fixed a) { return a.sqrt(); }
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef TARGET_IS_NDS
|
||||
// Use the libnds lookup tables for trigonometry functions
|
||||
inline Fixed Fixed::cosf() {
|
||||
int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
|
||||
if(idx < 0)
|
||||
idx += 512;
|
||||
return Fixed(RAW, COS_bin[idx] << 4);
|
||||
}
|
||||
inline Fixed cosf(Fixed x) { return x.cosf(); }
|
||||
inline Fixed Fixed::sinf() {
|
||||
int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
|
||||
if(idx < 0)
|
||||
idx += 512;
|
||||
return Fixed(RAW, SIN_bin[idx] << 4);
|
||||
}
|
||||
inline Fixed sinf(Fixed x) { return x.sinf(); }
|
||||
inline Fixed Fixed::tanf() {
|
||||
int idx = (((long long)g*(long long)G_1_DIV_PI)>>24)%512;
|
||||
if(idx < 0)
|
||||
idx += 512;
|
||||
return Fixed(RAW, TAN_bin[idx] << 4);
|
||||
}
|
||||
inline Fixed tanf(Fixed x) { return x.tanf(); }
|
||||
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,59 @@
|
||||
/*
|
||||
* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
|
||||
*
|
||||
* 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 "b2Settings.h"
|
||||
|
||||
const int32 b2_chunkSize = 4096;
|
||||
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();
|
||||
|
||||
void* Allocate(int32 size);
|
||||
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
|
||||
@@ -0,0 +1,636 @@
|
||||
/*
|
||||
* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
|
||||
*
|
||||
* 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 "b2Settings.h"
|
||||
#include <cmath>
|
||||
#include <cfloat>
|
||||
#include <cstdlib>
|
||||
|
||||
#include <stdio.h>
|
||||
|
||||
#ifdef TARGET_FLOAT32_IS_FIXED
|
||||
|
||||
inline Fixed b2Min(const Fixed& a, const Fixed& b)
|
||||
{
|
||||
return a < b ? a : b;
|
||||
}
|
||||
|
||||
inline Fixed b2Max(const Fixed& a, const Fixed& b)
|
||||
{
|
||||
return a > b ? a : b;
|
||||
}
|
||||
|
||||
inline Fixed b2Clamp(Fixed a, Fixed low, Fixed high)
|
||||
{
|
||||
return b2Max(low, b2Min(a, high));
|
||||
}
|
||||
|
||||
inline bool b2IsValid(Fixed x)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
|
||||
#define b2Sqrt(x) sqrt(x)
|
||||
#define b2Atan2(y, x) atan2(y, x)
|
||||
|
||||
#else
|
||||
|
||||
/// This function is used to ensure that a floating point number is
|
||||
/// not a NaN or infinity.
|
||||
inline bool b2IsValid(float32 x)
|
||||
{
|
||||
#ifdef _MSC_VER
|
||||
return _finite(x) != 0;
|
||||
#else
|
||||
return finite(x) != 0;
|
||||
#endif
|
||||
}
|
||||
|
||||
/// 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)
|
||||
|
||||
#endif
|
||||
|
||||
inline float32 b2Abs(float32 a)
|
||||
{
|
||||
return a > 0.0f ? a : -a;
|
||||
}
|
||||
|
||||
/// 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; }
|
||||
|
||||
/// 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
|
||||
{
|
||||
#ifdef TARGET_FLOAT32_IS_FIXED
|
||||
float est = b2Abs(x) + b2Abs(y);
|
||||
if(est == 0.0f) {
|
||||
return 0.0;
|
||||
} else if(est < 0.1) {
|
||||
return (1.0/256.0) * b2Vec2(x<<8, y<<8).Length();
|
||||
} else if(est < 180.0f) {
|
||||
return b2Sqrt(x * x + y * y);
|
||||
} else {
|
||||
return 256.0 * (b2Vec2(x>>8, y>>8).Length());
|
||||
}
|
||||
#else
|
||||
return b2Sqrt(x * x + y * y);
|
||||
#endif
|
||||
}
|
||||
|
||||
/// 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.
|
||||
#ifdef TARGET_FLOAT32_IS_FIXED
|
||||
float32 Normalize()
|
||||
{
|
||||
float32 length = Length();
|
||||
if (length < B2_FLT_EPSILON)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
#ifdef NORMALIZE_BY_INVERT_MULTIPLY
|
||||
if (length < (1.0/16.0)) {
|
||||
x = x << 4;
|
||||
y = y << 4;
|
||||
return (1.0/16.0)*Normalize();
|
||||
} else if(length > 16.0) {
|
||||
x = x >> 4;
|
||||
y = y >> 4;
|
||||
return 16.0*Normalize();
|
||||
}
|
||||
float32 invLength = 1.0f / length;
|
||||
x *= invLength;
|
||||
y *= invLength;
|
||||
#else
|
||||
x /= length;
|
||||
y /= length;
|
||||
#endif
|
||||
return length;
|
||||
}
|
||||
#else
|
||||
float32 Normalize()
|
||||
{
|
||||
float32 length = Length();
|
||||
if (length < B2_FLT_EPSILON)
|
||||
{
|
||||
return 0.0f;
|
||||
}
|
||||
float32 invLength = 1.0f / length;
|
||||
x *= invLength;
|
||||
y *= invLength;
|
||||
|
||||
return length;
|
||||
}
|
||||
#endif
|
||||
|
||||
/// Does this vector contain finite coordinates?
|
||||
bool IsValid() const
|
||||
{
|
||||
return b2IsValid(x) && b2IsValid(y);
|
||||
}
|
||||
|
||||
float32 x, y;
|
||||
};
|
||||
|
||||
/// 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)
|
||||
{
|
||||
col1 = c1;
|
||||
col2 = c2;
|
||||
}
|
||||
|
||||
/// Construct this matrix using scalars.
|
||||
b2Mat22(float32 a11, float32 a12, float32 a21, float32 a22)
|
||||
{
|
||||
col1.x = a11; col1.y = a21;
|
||||
col2.x = a12; col2.y = a22;
|
||||
}
|
||||
|
||||
/// Construct this matrix using an angle. This matrix becomes
|
||||
/// an orthonormal rotation matrix.
|
||||
explicit b2Mat22(float32 angle)
|
||||
{
|
||||
float32 c = cosf(angle), s = sinf(angle);
|
||||
col1.x = c; col2.x = -s;
|
||||
col1.y = s; col2.y = c;
|
||||
}
|
||||
|
||||
/// Initialize this matrix using columns.
|
||||
void Set(const b2Vec2& c1, const b2Vec2& c2)
|
||||
{
|
||||
col1 = c1;
|
||||
col2 = c2;
|
||||
}
|
||||
|
||||
/// Initialize this matrix using an angle. This matrix becomes
|
||||
/// an orthonormal rotation matrix.
|
||||
void Set(float32 angle)
|
||||
{
|
||||
float32 c = cosf(angle), s = sinf(angle);
|
||||
col1.x = c; col2.x = -s;
|
||||
col1.y = s; col2.y = c;
|
||||
}
|
||||
|
||||
/// Set this to the identity matrix.
|
||||
void SetIdentity()
|
||||
{
|
||||
col1.x = 1.0f; col2.x = 0.0f;
|
||||
col1.y = 0.0f; col2.y = 1.0f;
|
||||
}
|
||||
|
||||
/// Set this matrix to all zeros.
|
||||
void SetZero()
|
||||
{
|
||||
col1.x = 0.0f; col2.x = 0.0f;
|
||||
col1.y = 0.0f; col2.y = 0.0f;
|
||||
}
|
||||
|
||||
/// Extract the angle from this matrix (assumed to be
|
||||
/// a rotation matrix).
|
||||
float32 GetAngle() const
|
||||
{
|
||||
return b2Atan2(col1.y, col1.x);
|
||||
}
|
||||
|
||||
#ifdef TARGET_FLOAT32_IS_FIXED
|
||||
|
||||
/// Compute the inverse of this matrix, such that inv(A) * A = identity.
|
||||
b2Mat22 Invert() const
|
||||
{
|
||||
float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
|
||||
float32 det = a * d - b * c;
|
||||
b2Mat22 B;
|
||||
int n = 0;
|
||||
|
||||
if(b2Abs(det) <= (B2_FLT_EPSILON<<8))
|
||||
{
|
||||
n = 3;
|
||||
a = a<<n; b = b<<n;
|
||||
c = c<<n; d = d<<n;
|
||||
det = a * d - b * c;
|
||||
b2Assert(det != 0.0f);
|
||||
det = float32(1) / det;
|
||||
B.col1.x = ( det * d) << n; B.col2.x = (-det * b) << n;
|
||||
B.col1.y = (-det * c) << n; B.col2.y = ( det * a) << n;
|
||||
}
|
||||
else
|
||||
{
|
||||
n = (b2Abs(det) >= 16.0)? 4 : 0;
|
||||
b2Assert(det != 0.0f);
|
||||
det = float32(1<<n) / det;
|
||||
B.col1.x = ( det * d) >> n; B.col2.x = (-det * b) >> n;
|
||||
B.col1.y = (-det * c) >> n; B.col2.y = ( det * a) >> n;
|
||||
}
|
||||
|
||||
return B;
|
||||
}
|
||||
|
||||
// Solve A * x = b
|
||||
b2Vec2 Solve(const b2Vec2& b) const
|
||||
{
|
||||
float32 a11 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
|
||||
float32 det = a11 * a22 - a12 * a21;
|
||||
int n = 0;
|
||||
b2Vec2 x;
|
||||
|
||||
|
||||
if(b2Abs(det) <= (B2_FLT_EPSILON<<8))
|
||||
{
|
||||
n = 3;
|
||||
a11 = col1.x<<n; a12 = col2.x<<n;
|
||||
a21 = col1.y<<n; a22 = col2.y<<n;
|
||||
det = a11 * a22 - a12 * a21;
|
||||
b2Assert(det != 0.0f);
|
||||
det = float32(1) / det;
|
||||
x.x = (det * (a22 * b.x - a12 * b.y)) << n;
|
||||
x.y = (det * (a11 * b.y - a21 * b.x)) << n;
|
||||
}
|
||||
else
|
||||
{
|
||||
n = (b2Abs(det) >= 16.0) ? 4 : 0;
|
||||
b2Assert(det != 0.0f);
|
||||
det = float32(1<<n) / det;
|
||||
x.x = (det * (a22 * b.x - a12 * b.y)) >> n;
|
||||
x.y = (det * (a11 * b.y - a21 * b.x)) >> n;
|
||||
}
|
||||
|
||||
return x;
|
||||
}
|
||||
|
||||
#else
|
||||
b2Mat22 Invert() const
|
||||
{
|
||||
float32 a = col1.x, b = col2.x, c = col1.y, d = col2.y;
|
||||
b2Mat22 B;
|
||||
float32 det = a * d - b * c;
|
||||
b2Assert(det != 0.0f);
|
||||
det = float32(1.0f) / det;
|
||||
B.col1.x = det * d; B.col2.x = -det * b;
|
||||
B.col1.y = -det * c; B.col2.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 = col1.x, a12 = col2.x, a21 = col1.y, a22 = col2.y;
|
||||
float32 det = a11 * a22 - a12 * a21;
|
||||
b2Assert(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;
|
||||
}
|
||||
#endif
|
||||
|
||||
b2Vec2 col1, col2;
|
||||
};
|
||||
|
||||
/// A transform contains translation and rotation. It is used to represent
|
||||
/// the position and orientation of rigid frames.
|
||||
struct b2XForm
|
||||
{
|
||||
/// The default constructor does nothing (for performance).
|
||||
b2XForm() {}
|
||||
|
||||
/// Initialize using a position vector and a rotation matrix.
|
||||
b2XForm(const b2Vec2& position, const b2Mat22& R) : position(position), R(R) {}
|
||||
|
||||
/// Set this to the identity transform.
|
||||
void SetIdentity()
|
||||
{
|
||||
position.SetZero();
|
||||
R.SetIdentity();
|
||||
}
|
||||
|
||||
b2Vec2 position;
|
||||
b2Mat22 R;
|
||||
};
|
||||
|
||||
/// 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 t the normalized time in [0,1].
|
||||
void GetXForm(b2XForm* xf, float32 t) const;
|
||||
|
||||
/// Advance the sweep forward, yielding a new initial state.
|
||||
/// @param t the new initial time.
|
||||
void Advance(float32 t);
|
||||
|
||||
b2Vec2 localCenter; ///< local center of mass position
|
||||
b2Vec2 c0, c; ///< center world positions
|
||||
float32 a0, a; ///< world angles
|
||||
float32 t0; ///< time interval = [t0,1], where t0 is in [0,1]
|
||||
};
|
||||
|
||||
|
||||
extern const b2Vec2 b2Vec2_zero;
|
||||
extern const b2Mat22 b2Mat22_identity;
|
||||
extern const b2XForm b2XForm_identity;
|
||||
|
||||
/// Peform 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)
|
||||
{
|
||||
b2Vec2 v; v.Set(s * a.y, -s * a.x);
|
||||
return v;
|
||||
}
|
||||
|
||||
/// Perform the cross product on a scalar and a vector. In 2D this produces
|
||||
/// a vector.
|
||||
inline b2Vec2 b2Cross(float32 s, const b2Vec2& a)
|
||||
{
|
||||
b2Vec2 v; v.Set(-s * a.y, s * a.x);
|
||||
return v;
|
||||
}
|
||||
|
||||
/// 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)
|
||||
{
|
||||
b2Vec2 u;
|
||||
u.Set(A.col1.x * v.x + A.col2.x * v.y, A.col1.y * v.x + A.col2.y * v.y);
|
||||
return u;
|
||||
}
|
||||
|
||||
/// 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)
|
||||
{
|
||||
b2Vec2 u;
|
||||
u.Set(b2Dot(v, A.col1), b2Dot(v, A.col2));
|
||||
return u;
|
||||
}
|
||||
|
||||
/// Add two vectors component-wise.
|
||||
inline b2Vec2 operator + (const b2Vec2& a, const b2Vec2& b)
|
||||
{
|
||||
b2Vec2 v; v.Set(a.x + b.x, a.y + b.y);
|
||||
return v;
|
||||
}
|
||||
|
||||
/// Subtract two vectors component-wise.
|
||||
inline b2Vec2 operator - (const b2Vec2& a, const b2Vec2& b)
|
||||
{
|
||||
b2Vec2 v; v.Set(a.x - b.x, a.y - b.y);
|
||||
return v;
|
||||
}
|
||||
|
||||
inline b2Vec2 operator * (float32 s, const b2Vec2& a)
|
||||
{
|
||||
b2Vec2 v; v.Set(s * a.x, s * a.y);
|
||||
return v;
|
||||
}
|
||||
|
||||
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 b2Mat22 operator + (const b2Mat22& A, const b2Mat22& B)
|
||||
{
|
||||
b2Mat22 C;
|
||||
C.Set(A.col1 + B.col1, A.col2 + B.col2);
|
||||
return C;
|
||||
}
|
||||
|
||||
// A * B
|
||||
inline b2Mat22 b2Mul(const b2Mat22& A, const b2Mat22& B)
|
||||
{
|
||||
b2Mat22 C;
|
||||
C.Set(b2Mul(A, B.col1), b2Mul(A, B.col2));
|
||||
return C;
|
||||
}
|
||||
|
||||
// A^T * B
|
||||
inline b2Mat22 b2MulT(const b2Mat22& A, const b2Mat22& B)
|
||||
{
|
||||
b2Vec2 c1; c1.Set(b2Dot(A.col1, B.col1), b2Dot(A.col2, B.col1));
|
||||
b2Vec2 c2; c2.Set(b2Dot(A.col1, B.col2), b2Dot(A.col2, B.col2));
|
||||
b2Mat22 C;
|
||||
C.Set(c1, c2);
|
||||
return C;
|
||||
}
|
||||
|
||||
inline b2Vec2 b2Mul(const b2XForm& T, const b2Vec2& v)
|
||||
{
|
||||
return T.position + b2Mul(T.R, v);
|
||||
}
|
||||
|
||||
inline b2Vec2 b2MulT(const b2XForm& T, const b2Vec2& v)
|
||||
{
|
||||
return b2MulT(T.R, v - T.position);
|
||||
}
|
||||
|
||||
inline b2Vec2 b2Abs(const b2Vec2& a)
|
||||
{
|
||||
b2Vec2 b; b.Set(b2Abs(a.x), b2Abs(a.y));
|
||||
return b;
|
||||
}
|
||||
|
||||
inline b2Mat22 b2Abs(const b2Mat22& A)
|
||||
{
|
||||
b2Mat22 B;
|
||||
B.Set(b2Abs(A.col1), b2Abs(A.col2));
|
||||
return B;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline T b2Min(T a, T b)
|
||||
{
|
||||
return a < b ? a : b;
|
||||
}
|
||||
|
||||
inline b2Vec2 b2Min(const b2Vec2& a, const b2Vec2& b)
|
||||
{
|
||||
b2Vec2 c;
|
||||
c.x = b2Min(a.x, b.x);
|
||||
c.y = b2Min(a.y, b.y);
|
||||
return c;
|
||||
}
|
||||
|
||||
template <typename T>
|
||||
inline T b2Max(T a, T b)
|
||||
{
|
||||
return a > b ? a : b;
|
||||
}
|
||||
|
||||
inline b2Vec2 b2Max(const b2Vec2& a, const b2Vec2& b)
|
||||
{
|
||||
b2Vec2 c;
|
||||
c.x = b2Max(a.x, b.x);
|
||||
c.y = b2Max(a.y, b.y);
|
||||
return c;
|
||||
}
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
#define RAND_LIMIT 32767
|
||||
|
||||
// Random number in range [-1,1]
|
||||
inline float32 b2Random()
|
||||
{
|
||||
float32 r = (float32)(rand() & (RAND_LIMIT));
|
||||
r /= RAND_LIMIT;
|
||||
r = 2.0f * r - 1.0f;
|
||||
return r;
|
||||
}
|
||||
|
||||
/// Random floating point number in range [lo, hi]
|
||||
inline float32 b2Random(float32 lo, float32 hi)
|
||||
{
|
||||
float32 r = (float32)(rand() & (RAND_LIMIT));
|
||||
r /= RAND_LIMIT;
|
||||
r = (hi - lo) * r + lo;
|
||||
return r;
|
||||
}
|
||||
|
||||
/// "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;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,176 @@
|
||||
/*
|
||||
* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
|
||||
*
|
||||
* 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 <assert.h>
|
||||
#include <math.h>
|
||||
|
||||
#define B2_NOT_USED(x) x
|
||||
#define b2Assert(A) assert(A)
|
||||
|
||||
|
||||
// need to include NDS jtypes.h instead of
|
||||
// usual typedefs because NDS jtypes defines
|
||||
// them slightly differently, oh well.
|
||||
#ifdef TARGET_IS_NDS
|
||||
|
||||
#include "jtypes.h"
|
||||
|
||||
#else
|
||||
|
||||
typedef signed char int8;
|
||||
typedef signed short int16;
|
||||
typedef signed int int32;
|
||||
typedef unsigned char uint8;
|
||||
typedef unsigned short uint16;
|
||||
typedef unsigned int uint32;
|
||||
|
||||
#endif
|
||||
|
||||
#ifdef TARGET_FLOAT32_IS_FIXED
|
||||
|
||||
#include "Fixed.h"
|
||||
|
||||
typedef Fixed float32;
|
||||
#define B2_FLT_MAX FIXED_MAX
|
||||
#define B2_FLT_EPSILON FIXED_EPSILON
|
||||
#define B2FORCE_SCALE(x) ((x)<<7)
|
||||
#define B2FORCE_INV_SCALE(x) ((x)>>7)
|
||||
|
||||
#else
|
||||
|
||||
typedef float float32;
|
||||
#define B2_FLT_MAX FLT_MAX
|
||||
#define B2_FLT_EPSILON FLT_EPSILON
|
||||
#define B2FORCE_SCALE(x) (x)
|
||||
#define B2FORCE_INV_SCALE(x) (x)
|
||||
|
||||
#endif
|
||||
|
||||
const float32 b2_pi = 3.14159265359f;
|
||||
|
||||
/// @file
|
||||
/// Global tuning constants based on meters-kilograms-seconds (MKS) units.
|
||||
///
|
||||
|
||||
// Collision
|
||||
const int32 b2_maxManifoldPoints = 2;
|
||||
const int32 b2_maxPolygonVertices = 8;
|
||||
const int32 b2_maxProxies = 16384; // this must be a power of two
|
||||
const int32 b2_maxPairs = 8 * b2_maxProxies; // this must be a power of two
|
||||
|
||||
// Dynamics
|
||||
|
||||
/// A small length used as a collision and constraint tolerance. Usually it is
|
||||
/// chosen to be numerically significant, but visually insignificant.
|
||||
const float32 b2_linearSlop = 0.005f; // 0.5 cm
|
||||
|
||||
/// A small angle used as a collision and constraint tolerance. Usually it is
|
||||
/// chosen to be numerically significant, but visually insignificant.
|
||||
const float32 b2_angularSlop = 2.0f / 180.0f * b2_pi; // 2 degrees
|
||||
|
||||
/// Continuous collision detection (CCD) works with core, shrunken shapes. This is the
|
||||
/// amount by which shapes are automatically shrunk to work with CCD. This must be
|
||||
/// larger than b2_linearSlop.
|
||||
const float32 b2_toiSlop = 8.0f * b2_linearSlop;
|
||||
|
||||
/// Maximum number of contacts to be handled to solve a TOI island.
|
||||
const int32 b2_maxTOIContactsPerIsland = 32;
|
||||
|
||||
/// A velocity threshold for elastic collisions. Any collision with a relative linear
|
||||
/// velocity below this threshold will be treated as inelastic.
|
||||
const float32 b2_velocityThreshold = 1.0f; // 1 m/s
|
||||
|
||||
/// The maximum linear position correction used when solving constraints. This helps to
|
||||
/// prevent overshoot.
|
||||
const float32 b2_maxLinearCorrection = 0.2f; // 20 cm
|
||||
|
||||
/// The maximum angular position correction used when solving constraints. This helps to
|
||||
/// prevent overshoot.
|
||||
const float32 b2_maxAngularCorrection = 8.0f / 180.0f * b2_pi; // 8 degrees
|
||||
|
||||
/// 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.
|
||||
#ifdef TARGET_FLOAT32_IS_FIXED
|
||||
const float32 b2_maxLinearVelocity = 100.0f;
|
||||
#else
|
||||
const float32 b2_maxLinearVelocity = 200.0f;
|
||||
const float32 b2_maxLinearVelocitySquared = b2_maxLinearVelocity * b2_maxLinearVelocity;
|
||||
#endif
|
||||
|
||||
/// 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.
|
||||
const float32 b2_maxAngularVelocity = 250.0f;
|
||||
#ifndef TARGET_FLOAT32_IS_FIXED
|
||||
const float32 b2_maxAngularVelocitySquared = b2_maxAngularVelocity * b2_maxAngularVelocity;
|
||||
#endif
|
||||
|
||||
/// 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.
|
||||
const float32 b2_contactBaumgarte = 0.2f;
|
||||
|
||||
// Sleep
|
||||
|
||||
/// The time that a body must be still before it will go to sleep.
|
||||
const float32 b2_timeToSleep = 0.5f; // half a second
|
||||
|
||||
/// A body cannot sleep if its linear velocity is above this tolerance.
|
||||
const float32 b2_linearSleepTolerance = 0.01f; // 1 cm/s
|
||||
|
||||
/// A body cannot sleep if its angular velocity is above this tolerance.
|
||||
const float32 b2_angularSleepTolerance = 2.0f / 180.0f; // 2 degrees/s
|
||||
|
||||
// Memory Allocation
|
||||
|
||||
/// The current number of bytes allocated through b2Alloc.
|
||||
extern int32 b2_byteCount;
|
||||
|
||||
/// 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);
|
||||
|
||||
/// 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;
|
||||
|
||||
/// Friction mixing law. Feel free to customize this.
|
||||
inline float32 b2MixFriction(float32 friction1, float32 friction2)
|
||||
{
|
||||
return sqrtf(friction1 * friction2);
|
||||
}
|
||||
|
||||
/// Restitution mixing law. Feel free to customize this.
|
||||
inline float32 b2MixRestitution(float32 restitution1, float32 restitution2)
|
||||
{
|
||||
return restitution1 > restitution2 ? restitution1 : restitution2;
|
||||
}
|
||||
|
||||
#endif
|
||||
@@ -0,0 +1,60 @@
|
||||
/*
|
||||
* Copyright (c) 2006-2007 Erin Catto http://www.gphysics.com
|
||||
*
|
||||
* 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 "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
|
||||
@@ -0,0 +1,139 @@
|
||||
/*---------------------------------------------------------------------------------
|
||||
$Id: jtypes.h,v 1.17 2007/07/18 05:20:45 wntrmute Exp $
|
||||
|
||||
jtypes.h -- Common types (and a few useful macros)
|
||||
|
||||
Copyright (C) 2005
|
||||
Michael Noland (joat)
|
||||
Jason Rogers (dovoto)
|
||||
Dave Murphy (WinterMute)
|
||||
Chris Double (doublec)
|
||||
|
||||
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 NDS_JTYPES_INCLUDE
|
||||
#define NDS_JTYPES_INCLUDE
|
||||
//---------------------------------------------------------------------------------
|
||||
|
||||
|
||||
#define PACKED __attribute__ ((packed))
|
||||
#define packed_struct struct PACKED
|
||||
|
||||
//---------------------------------------------------------------------------------
|
||||
// libgba compatible section macros
|
||||
//---------------------------------------------------------------------------------
|
||||
#define ITCM_CODE __attribute__((section(".itcm"), long_call))
|
||||
|
||||
#define DTCM_DATA __attribute__((section(".dtcm")))
|
||||
#define DTCM_BSS __attribute__((section(".sbss")))
|
||||
#define ALIGN(m) __attribute__((aligned (m)))
|
||||
|
||||
#define PACKED __attribute__ ((packed))
|
||||
#define packed_struct struct PACKED
|
||||
|
||||
//---------------------------------------------------------------------------------
|
||||
// These are linked to the bin2o macro in the Makefile
|
||||
//---------------------------------------------------------------------------------
|
||||
#define GETRAW(name) (name)
|
||||
#define GETRAWSIZE(name) ((int)name##_size)
|
||||
#define GETRAWEND(name) ((int)name##_end)
|
||||
|
||||
#ifndef TRUE
|
||||
#define TRUE 1
|
||||
#define FALSE 0
|
||||
#endif
|
||||
|
||||
#define BIT(n) (1 << (n))
|
||||
|
||||
// define libnds types in terms of stdint
|
||||
#include <stdint.h>
|
||||
|
||||
typedef uint8_t uint8;
|
||||
typedef uint16_t uint16;
|
||||
typedef uint32_t uint32;
|
||||
typedef uint64_t uint64;
|
||||
|
||||
typedef int8_t int8;
|
||||
typedef int16_t int16;
|
||||
typedef int32_t int32;
|
||||
typedef int64_t int64;
|
||||
|
||||
//typedef float float32;
|
||||
typedef double float64;
|
||||
|
||||
typedef volatile uint8_t vuint8;
|
||||
typedef volatile uint16_t vuint16;
|
||||
typedef volatile uint32_t vuint32;
|
||||
typedef volatile uint64_t vuint64;
|
||||
|
||||
typedef volatile int8_t vint8;
|
||||
typedef volatile int16_t vint16;
|
||||
typedef volatile int32_t vint32;
|
||||
typedef volatile int64_t vint64;
|
||||
|
||||
typedef volatile float vfloat32;
|
||||
typedef volatile float64 vfloat64;
|
||||
|
||||
typedef uint8_t byte;
|
||||
|
||||
typedef uint8_t u8;
|
||||
typedef uint16_t u16;
|
||||
typedef uint32_t u32;
|
||||
typedef uint64_t u64;
|
||||
|
||||
typedef int8_t s8;
|
||||
typedef int16_t s16;
|
||||
typedef int32_t s32;
|
||||
typedef int64_t s64;
|
||||
|
||||
typedef volatile u8 vu8;
|
||||
typedef volatile u16 vu16;
|
||||
typedef volatile u32 vu32;
|
||||
typedef volatile u64 vu64;
|
||||
|
||||
typedef volatile s8 vs8;
|
||||
typedef volatile s16 vs16;
|
||||
typedef volatile s32 vs32;
|
||||
typedef volatile s64 vs64;
|
||||
|
||||
typedef struct touchPosition {
|
||||
int16 x;
|
||||
int16 y;
|
||||
int16 px;
|
||||
int16 py;
|
||||
int16 z1;
|
||||
int16 z2;
|
||||
} touchPosition;
|
||||
|
||||
|
||||
#ifndef __cplusplus
|
||||
/** C++ compatible bool for C
|
||||
|
||||
*/
|
||||
typedef enum { false, true } bool;
|
||||
#endif
|
||||
|
||||
// Handy function pointer typedefs
|
||||
typedef void ( * IntFn)(void);
|
||||
typedef void (* VoidFunctionPointer)(void);
|
||||
typedef void (* fp)(void);
|
||||
|
||||
//---------------------------------------------------------------------------------
|
||||
#endif
|
||||
//---------------------------------------------------------------------------------
|
||||
Reference in New Issue
Block a user