Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

Code-style consistency improvement:

Browse Source 
Apply clang-format-all.sh using the _clang-format file through all the cpp/.h files.
make sure not to apply it to certain serialization structures, since some parser expects the * as part of the name, instead of type.
This commit contains no other changes aside from adding and applying clang-format-all.sh
This commit is contained in:
erwincoumans
2018-09-23 14:17:31 -07:00
parent b73b05e9fb
commit ab8f16961e
1773 changed files with 1081087 additions and 474249 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1362
src/LinearMath/TaskScheduler/btTaskScheduler.cpp
View File

File diff suppressed because it is too large Load Diff

64
src/LinearMath/TaskScheduler/btThreadSupportInterface.h
View File

@@ -16,55 +16,49 @@ subject to the following restrictions:
#ifndef BT_THREAD_SUPPORT_INTERFACE_H
#define BT_THREAD_SUPPORT_INTERFACE_H
class btCriticalSection
{
public:
btCriticalSection() {}
virtual ~btCriticalSection() {}
btCriticalSection() {}
virtual ~btCriticalSection() {}
virtual void lock() = 0;
virtual void unlock() = 0;
virtual void lock() = 0;
virtual void unlock() = 0;
};
class btThreadSupportInterface
{
public:
virtual ~btThreadSupportInterface() {}
virtual ~btThreadSupportInterface() {}
virtual int getNumWorkerThreads() const = 0; // number of worker threads (total number of logical processors - 1)
virtual int getCacheFriendlyNumThreads() const = 0; // the number of logical processors sharing a single L3 cache
virtual int getLogicalToPhysicalCoreRatio() const = 0; // the number of logical processors per physical processor (usually 1 or 2)
virtual void runTask(int threadIndex, void* userData) = 0;
virtual void waitForAllTasks() = 0;
virtual int getNumWorkerThreads() const = 0; // number of worker threads (total number of logical processors - 1)
virtual int getCacheFriendlyNumThreads() const = 0; // the number of logical processors sharing a single L3 cache
virtual int getLogicalToPhysicalCoreRatio() const = 0; // the number of logical processors per physical processor (usually 1 or 2)
virtual void runTask( int threadIndex, void* userData ) = 0;
virtual void waitForAllTasks() = 0;
virtual btCriticalSection* createCriticalSection() = 0;
virtual void deleteCriticalSection(btCriticalSection* criticalSection) = 0;
virtual btCriticalSection* createCriticalSection() = 0;
virtual void deleteCriticalSection( btCriticalSection* criticalSection ) = 0;
typedef void (*ThreadFunc)(void* userPtr);
typedef void( *ThreadFunc )( void* userPtr );
struct ConstructionInfo
{
ConstructionInfo(const char* uniqueName,
ThreadFunc userThreadFunc,
int threadStackSize = 65535)
: m_uniqueName(uniqueName),
m_userThreadFunc(userThreadFunc),
m_threadStackSize(threadStackSize)
{
}
struct ConstructionInfo
{
ConstructionInfo( const char* uniqueName,
ThreadFunc userThreadFunc,
int threadStackSize = 65535
)
:m_uniqueName( uniqueName ),
m_userThreadFunc( userThreadFunc ),
m_threadStackSize( threadStackSize )
{
}
const char* m_uniqueName;
ThreadFunc m_userThreadFunc;
int m_threadStackSize;
};
const char* m_uniqueName;
ThreadFunc m_userThreadFunc;
int m_threadStackSize;
};
static btThreadSupportInterface* create( const ConstructionInfo& info );
static btThreadSupportInterface* create(const ConstructionInfo& info);
};
#endif //BT_THREAD_SUPPORT_INTERFACE_H
#endif //BT_THREAD_SUPPORT_INTERFACE_H

407
src/LinearMath/TaskScheduler/btThreadSupportPosix.cpp
View File

@@ -13,9 +13,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#if BT_THREADSAFE && !defined( _WIN32 )
#if BT_THREADSAFE && !defined(_WIN32)
#include "LinearMath/btScalar.h"
#include "LinearMath/btAlignedObjectArray.h"
@@ -27,14 +25,12 @@ subject to the following restrictions:
#include <errno.h>
#include <unistd.h>
#ifndef _XOPEN_SOURCE
#define _XOPEN_SOURCE 600 //for definition of pthread_barrier_t, see http://pages.cs.wisc.edu/~travitch/pthreads_primer.html
#endif //_XOPEN_SOURCE
#define _XOPEN_SOURCE 600 //for definition of pthread_barrier_t, see http://pages.cs.wisc.edu/~travitch/pthreads_primer.html
#endif //_XOPEN_SOURCE
#include <pthread.h>
#include <semaphore.h>
#include <unistd.h> //for sysconf
#include <unistd.h> //for sysconf
///
/// getNumHardwareThreads()
@@ -48,318 +44,309 @@ subject to the following restrictions:
int btGetNumHardwareThreads()
{
return btMin<int>(BT_MAX_THREAD_COUNT, std::thread::hardware_concurrency());
return btMin<int>(BT_MAX_THREAD_COUNT, std::thread::hardware_concurrency());
}
#else
int btGetNumHardwareThreads()
{
return btMin<int>(BT_MAX_THREAD_COUNT, sysconf( _SC_NPROCESSORS_ONLN ));
return btMin<int>(BT_MAX_THREAD_COUNT, sysconf(_SC_NPROCESSORS_ONLN));
}
#endif
// btThreadSupportPosix helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
class btThreadSupportPosix : public btThreadSupportInterface
{
public:
struct btThreadStatus
{
int m_taskId;
int m_commandId;
int m_status;
struct btThreadStatus
{
int m_taskId;
int m_commandId;
int m_status;
ThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc
ThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc
pthread_t thread;
//each tread will wait until this signal to start its work
sem_t* startSemaphore;
pthread_t thread;
//each tread will wait until this signal to start its work
sem_t* startSemaphore;
// this is a copy of m_mainSemaphore,
//each tread will signal once it is finished with its work
sem_t* m_mainSemaphore;
unsigned long threadUsed;
};
// this is a copy of m_mainSemaphore,
//each tread will signal once it is finished with its work
sem_t* m_mainSemaphore;
unsigned long threadUsed;
};
private:
typedef unsigned long long UINT64;
typedef unsigned long long UINT64;
btAlignedObjectArray<btThreadStatus> m_activeThreadStatus;
// m_mainSemaphoresemaphore will signal, if and how many threads are finished with their work
sem_t* m_mainSemaphore;
int m_numThreads;
UINT64 m_startedThreadsMask;
void startThreads( const ConstructionInfo& threadInfo );
void stopThreads();
int waitForResponse();
btAlignedObjectArray<btThreadStatus> m_activeThreadStatus;
// m_mainSemaphoresemaphore will signal, if and how many threads are finished with their work
sem_t* m_mainSemaphore;
int m_numThreads;
UINT64 m_startedThreadsMask;
void startThreads(const ConstructionInfo& threadInfo);
void stopThreads();
int waitForResponse();
public:
btThreadSupportPosix( const ConstructionInfo& threadConstructionInfo );
virtual ~btThreadSupportPosix();
btThreadSupportPosix(const ConstructionInfo& threadConstructionInfo);
virtual ~btThreadSupportPosix();
virtual int getNumWorkerThreads() const BT_OVERRIDE { return m_numThreads; }
// TODO: return the number of logical processors sharing the first L3 cache
virtual int getCacheFriendlyNumThreads() const BT_OVERRIDE { return m_numThreads + 1; }
// TODO: detect if CPU has hyperthreading enabled
virtual int getLogicalToPhysicalCoreRatio() const BT_OVERRIDE { return 1; }
virtual int getNumWorkerThreads() const BT_OVERRIDE { return m_numThreads; }
// TODO: return the number of logical processors sharing the first L3 cache
virtual int getCacheFriendlyNumThreads() const BT_OVERRIDE { return m_numThreads + 1; }
// TODO: detect if CPU has hyperthreading enabled
virtual int getLogicalToPhysicalCoreRatio() const BT_OVERRIDE { return 1; }
virtual void runTask( int threadIndex, void* userData ) BT_OVERRIDE;
virtual void waitForAllTasks() BT_OVERRIDE;
virtual void runTask(int threadIndex, void* userData) BT_OVERRIDE;
virtual void waitForAllTasks() BT_OVERRIDE;
virtual btCriticalSection* createCriticalSection() BT_OVERRIDE;
virtual void deleteCriticalSection( btCriticalSection* criticalSection ) BT_OVERRIDE;
virtual btCriticalSection* createCriticalSection() BT_OVERRIDE;
virtual void deleteCriticalSection(btCriticalSection* criticalSection) BT_OVERRIDE;
};
#define checkPThreadFunction(returnValue) \
if(0 != returnValue) { \
printf("PThread problem at line %i in file %s: %i %d\n", __LINE__, __FILE__, returnValue, errno); \
}
#define checkPThreadFunction(returnValue) \
if (0 != returnValue) \
{ \
printf("PThread problem at line %i in file %s: %i %d\n", __LINE__, __FILE__, returnValue, errno); \
}
// The number of threads should be equal to the number of available cores
// Todo: each worker should be linked to a single core, using SetThreadIdealProcessor.
btThreadSupportPosix::btThreadSupportPosix( const ConstructionInfo& threadConstructionInfo )
btThreadSupportPosix::btThreadSupportPosix(const ConstructionInfo& threadConstructionInfo)
{
startThreads( threadConstructionInfo );
startThreads(threadConstructionInfo);
}
// cleanup/shutdown Libspe2
btThreadSupportPosix::~btThreadSupportPosix()
{
stopThreads();
stopThreads();
}
#if (defined (__APPLE__))
#if (defined(__APPLE__))
#define NAMED_SEMAPHORES
#endif
static sem_t* createSem( const char* baseName )
static sem_t* createSem(const char* baseName)
{
static int semCount = 0;
static int semCount = 0;
#ifdef NAMED_SEMAPHORES
/// Named semaphore begin
char name[ 32 ];
snprintf( name, 32, "/%8.s-%4.d-%4.4d", baseName, getpid(), semCount++ );
sem_t* tempSem = sem_open( name, O_CREAT, 0600, 0 );
/// Named semaphore begin
char name[32];
snprintf(name, 32, "/%8.s-%4.d-%4.4d", baseName, getpid(), semCount++);
sem_t* tempSem = sem_open(name, O_CREAT, 0600, 0);
if ( tempSem != reinterpret_cast<sem_t *>( SEM_FAILED ) )
{
// printf("Created \"%s\" Semaphore %p\n", name, tempSem);
}
else
{
//printf("Error creating Semaphore %d\n", errno);
exit( -1 );
}
/// Named semaphore end
if (tempSem != reinterpret_cast<sem_t*>(SEM_FAILED))
{
// printf("Created \"%s\" Semaphore %p\n", name, tempSem);
}
else
{
//printf("Error creating Semaphore %d\n", errno);
exit(-1);
}
/// Named semaphore end
#else
sem_t* tempSem = new sem_t;
checkPThreadFunction( sem_init( tempSem, 0, 0 ) );
sem_t* tempSem = new sem_t;
checkPThreadFunction(sem_init(tempSem, 0, 0));
#endif
return tempSem;
return tempSem;
}
static void destroySem( sem_t* semaphore )
static void destroySem(sem_t* semaphore)
{
#ifdef NAMED_SEMAPHORES
checkPThreadFunction( sem_close( semaphore ) );
checkPThreadFunction(sem_close(semaphore));
#else
checkPThreadFunction( sem_destroy( semaphore ) );
delete semaphore;
checkPThreadFunction(sem_destroy(semaphore));
delete semaphore;
#endif
}
static void *threadFunction( void *argument )
static void* threadFunction(void* argument)
{
btThreadSupportPosix::btThreadStatus* status = ( btThreadSupportPosix::btThreadStatus* )argument;
btThreadSupportPosix::btThreadStatus* status = (btThreadSupportPosix::btThreadStatus*)argument;
while ( 1 )
{
checkPThreadFunction( sem_wait( status->startSemaphore ) );
void* userPtr = status->m_userPtr;
while (1)
{
checkPThreadFunction(sem_wait(status->startSemaphore));
void* userPtr = status->m_userPtr;
if ( userPtr )
{
btAssert( status->m_status );
status->m_userThreadFunc( userPtr );
status->m_status = 2;
checkPThreadFunction( sem_post( status->m_mainSemaphore ) );
status->threadUsed++;
}
else
{
//exit Thread
status->m_status = 3;
checkPThreadFunction( sem_post( status->m_mainSemaphore ) );
printf( "Thread with taskId %i exiting\n", status->m_taskId );
break;
}
}
if (userPtr)
{
btAssert(status->m_status);
status->m_userThreadFunc(userPtr);
status->m_status = 2;
checkPThreadFunction(sem_post(status->m_mainSemaphore));
status->threadUsed++;
}
else
{
//exit Thread
status->m_status = 3;
checkPThreadFunction(sem_post(status->m_mainSemaphore));
printf("Thread with taskId %i exiting\n", status->m_taskId);
break;
}
}
printf( "Thread TERMINATED\n" );
return 0;
printf("Thread TERMINATED\n");
return 0;
}
///send messages to SPUs
void btThreadSupportPosix::runTask( int threadIndex, void* userData )
void btThreadSupportPosix::runTask(int threadIndex, void* userData)
{
///we should spawn an SPU task here, and in 'waitForResponse' it should wait for response of the (one of) the first tasks that finished
btThreadStatus& threadStatus = m_activeThreadStatus[ threadIndex ];
btAssert( threadIndex >= 0 );
btAssert( threadIndex < m_activeThreadStatus.size() );
///we should spawn an SPU task here, and in 'waitForResponse' it should wait for response of the (one of) the first tasks that finished
btThreadStatus& threadStatus = m_activeThreadStatus[threadIndex];
btAssert(threadIndex >= 0);
btAssert(threadIndex < m_activeThreadStatus.size());
threadStatus.m_commandId = 1;
threadStatus.m_status = 1;
threadStatus.m_userPtr = userData;
m_startedThreadsMask |= UINT64( 1 ) << threadIndex;
threadStatus.m_commandId = 1;
threadStatus.m_status = 1;
threadStatus.m_userPtr = userData;
m_startedThreadsMask |= UINT64(1) << threadIndex;
// fire event to start new task
checkPThreadFunction( sem_post( threadStatus.startSemaphore ) );
// fire event to start new task
checkPThreadFunction(sem_post(threadStatus.startSemaphore));
}
///check for messages from SPUs
int btThreadSupportPosix::waitForResponse()
{
///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response
///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback'
///We should wait for (one of) the first tasks to finish (or other SPU messages), and report its response
///A possible response can be 'yes, SPU handled it', or 'no, please do a PPU fallback'
btAssert( m_activeThreadStatus.size() );
btAssert(m_activeThreadStatus.size());
// wait for any of the threads to finish
checkPThreadFunction( sem_wait( m_mainSemaphore ) );
// get at least one thread which has finished
size_t last = -1;
// wait for any of the threads to finish
checkPThreadFunction(sem_wait(m_mainSemaphore));
// get at least one thread which has finished
size_t last = -1;
for ( size_t t = 0; t < size_t( m_activeThreadStatus.size() ); ++t )
{
if ( 2 == m_activeThreadStatus[ t ].m_status )
{
last = t;
break;
}
}
for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t)
{
if (2 == m_activeThreadStatus[t].m_status)
{
last = t;
break;
}
}
btThreadStatus& threadStatus = m_activeThreadStatus[ last ];
btThreadStatus& threadStatus = m_activeThreadStatus[last];
btAssert( threadStatus.m_status > 1 );
threadStatus.m_status = 0;
btAssert(threadStatus.m_status > 1);
threadStatus.m_status = 0;
// need to find an active spu
btAssert( last >= 0 );
m_startedThreadsMask &= ~( UINT64( 1 ) << last );
// need to find an active spu
btAssert(last >= 0);
m_startedThreadsMask &= ~(UINT64(1) << last);
return last;
return last;
}
void btThreadSupportPosix::waitForAllTasks()
{
while ( m_startedThreadsMask )
{
waitForResponse();
}
while (m_startedThreadsMask)
{
waitForResponse();
}
}
void btThreadSupportPosix::startThreads( const ConstructionInfo& threadConstructionInfo )
void btThreadSupportPosix::startThreads(const ConstructionInfo& threadConstructionInfo)
{
m_numThreads = btGetNumHardwareThreads() - 1; // main thread exists already
printf( "%s creating %i threads.\n", __FUNCTION__, m_numThreads );
m_activeThreadStatus.resize( m_numThreads );
m_startedThreadsMask = 0;
m_numThreads = btGetNumHardwareThreads() - 1; // main thread exists already
printf("%s creating %i threads.\n", __FUNCTION__, m_numThreads);
m_activeThreadStatus.resize(m_numThreads);
m_startedThreadsMask = 0;
m_mainSemaphore = createSem( "main" );
//checkPThreadFunction(sem_wait(mainSemaphore));
m_mainSemaphore = createSem("main");
//checkPThreadFunction(sem_wait(mainSemaphore));
for ( int i = 0; i < m_numThreads; i++ )
{
printf( "starting thread %d\n", i );
btThreadStatus& threadStatus = m_activeThreadStatus[ i ];
threadStatus.startSemaphore = createSem( "threadLocal" );
checkPThreadFunction( pthread_create( &threadStatus.thread, NULL, &threadFunction, (void*) &threadStatus ) );
for (int i = 0; i < m_numThreads; i++)
{
printf("starting thread %d\n", i);
btThreadStatus& threadStatus = m_activeThreadStatus[i];
threadStatus.startSemaphore = createSem("threadLocal");
checkPThreadFunction(pthread_create(&threadStatus.thread, NULL, &threadFunction, (void*)&threadStatus));
threadStatus.m_userPtr = 0;
threadStatus.m_taskId = i;
threadStatus.m_commandId = 0;
threadStatus.m_status = 0;
threadStatus.m_mainSemaphore = m_mainSemaphore;
threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
threadStatus.threadUsed = 0;
threadStatus.m_userPtr = 0;
threadStatus.m_taskId = i;
threadStatus.m_commandId = 0;
threadStatus.m_status = 0;
threadStatus.m_mainSemaphore = m_mainSemaphore;
threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
threadStatus.threadUsed = 0;
printf( "started thread %d \n", i );
}
printf("started thread %d \n", i);
}
}
///tell the task scheduler we are done with the SPU tasks
void btThreadSupportPosix::stopThreads()
{
for ( size_t t = 0; t < size_t( m_activeThreadStatus.size() ); ++t )
{
btThreadStatus& threadStatus = m_activeThreadStatus[ t ];
printf( "%s: Thread %i used: %ld\n", __FUNCTION__, int( t ), threadStatus.threadUsed );
for (size_t t = 0; t < size_t(m_activeThreadStatus.size()); ++t)
{
btThreadStatus& threadStatus = m_activeThreadStatus[t];
printf("%s: Thread %i used: %ld\n", __FUNCTION__, int(t), threadStatus.threadUsed);
threadStatus.m_userPtr = 0;
checkPThreadFunction( sem_post( threadStatus.startSemaphore ) );
checkPThreadFunction( sem_wait( m_mainSemaphore ) );
threadStatus.m_userPtr = 0;
checkPThreadFunction(sem_post(threadStatus.startSemaphore));
checkPThreadFunction(sem_wait(m_mainSemaphore));
printf( "destroy semaphore\n" );
destroySem( threadStatus.startSemaphore );
printf( "semaphore destroyed\n" );
checkPThreadFunction( pthread_join( threadStatus.thread, 0 ) );
}
printf( "destroy main semaphore\n" );
destroySem( m_mainSemaphore );
printf( "main semaphore destroyed\n" );
m_activeThreadStatus.clear();
printf("destroy semaphore\n");
destroySem(threadStatus.startSemaphore);
printf("semaphore destroyed\n");
checkPThreadFunction(pthread_join(threadStatus.thread, 0));
}
printf("destroy main semaphore\n");
destroySem(m_mainSemaphore);
printf("main semaphore destroyed\n");
m_activeThreadStatus.clear();
}
class btCriticalSectionPosix : public btCriticalSection
{
pthread_mutex_t m_mutex;
pthread_mutex_t m_mutex;
public:
btCriticalSectionPosix()
{
pthread_mutex_init( &m_mutex, NULL );
}
virtual ~btCriticalSectionPosix()
{
pthread_mutex_destroy( &m_mutex );
}
btCriticalSectionPosix()
{
pthread_mutex_init(&m_mutex, NULL);
}
virtual ~btCriticalSectionPosix()
{
pthread_mutex_destroy(&m_mutex);
}
virtual void lock()
{
pthread_mutex_lock( &m_mutex );
}
virtual void unlock()
{
pthread_mutex_unlock( &m_mutex );
}
virtual void lock()
{
pthread_mutex_lock(&m_mutex);
}
virtual void unlock()
{
pthread_mutex_unlock(&m_mutex);
}
};
btCriticalSection* btThreadSupportPosix::createCriticalSection()
{
return new btCriticalSectionPosix();
return new btCriticalSectionPosix();
}
void btThreadSupportPosix::deleteCriticalSection( btCriticalSection* cs )
void btThreadSupportPosix::deleteCriticalSection(btCriticalSection* cs)
{
delete cs;
delete cs;
}
btThreadSupportInterface* btThreadSupportInterface::create( const ConstructionInfo& info )
btThreadSupportInterface* btThreadSupportInterface::create(const ConstructionInfo& info)
{
return new btThreadSupportPosix( info );
return new btThreadSupportPosix(info);
}
#endif // BT_THREADSAFE && !defined( _WIN32 )
#endif // BT_THREADSAFE && !defined( _WIN32 )

650
src/LinearMath/TaskScheduler/btThreadSupportWin32.cpp
View File

@@ -13,7 +13,7 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#if defined( _WIN32 ) && BT_THREADSAFE
#if defined(_WIN32) && BT_THREADSAFE
#include "LinearMath/btScalar.h"
#include "LinearMath/btMinMax.h"
@@ -23,450 +23,430 @@ subject to the following restrictions:
#include <windows.h>
#include <stdio.h>
struct btProcessorInfo
{
int numLogicalProcessors;
int numCores;
int numNumaNodes;
int numL1Cache;
int numL2Cache;
int numL3Cache;
int numPhysicalPackages;
static const int maxNumTeamMasks = 32;
int numTeamMasks;
UINT64 processorTeamMasks[ maxNumTeamMasks ];
int numLogicalProcessors;
int numCores;
int numNumaNodes;
int numL1Cache;
int numL2Cache;
int numL3Cache;
int numPhysicalPackages;
static const int maxNumTeamMasks = 32;
int numTeamMasks;
UINT64 processorTeamMasks[maxNumTeamMasks];
};
UINT64 getProcessorTeamMask( const btProcessorInfo& procInfo, int procId )
UINT64 getProcessorTeamMask(const btProcessorInfo& procInfo, int procId)
{
UINT64 procMask = UINT64( 1 ) << procId;
for ( int i = 0; i < procInfo.numTeamMasks; ++i )
{
if ( procMask & procInfo.processorTeamMasks[ i ] )
{
return procInfo.processorTeamMasks[ i ];
}
}
return 0;
UINT64 procMask = UINT64(1) << procId;
for (int i = 0; i < procInfo.numTeamMasks; ++i)
{
if (procMask & procInfo.processorTeamMasks[i])
{
return procInfo.processorTeamMasks[i];
}
}
return 0;
}
int getProcessorTeamIndex( const btProcessorInfo& procInfo, int procId )
int getProcessorTeamIndex(const btProcessorInfo& procInfo, int procId)
{
UINT64 procMask = UINT64( 1 ) << procId;
for ( int i = 0; i < procInfo.numTeamMasks; ++i )
{
if ( procMask & procInfo.processorTeamMasks[ i ] )
{
return i;
}
}
return -1;
UINT64 procMask = UINT64(1) << procId;
for (int i = 0; i < procInfo.numTeamMasks; ++i)
{
if (procMask & procInfo.processorTeamMasks[i])
{
return i;
}
}
return -1;
}
int countSetBits( ULONG64 bits )
int countSetBits(ULONG64 bits)
{
int count = 0;
while ( bits )
{
if ( bits & 1 )
{
count++;
}
bits >>= 1;
}
return count;
int count = 0;
while (bits)
{
if (bits & 1)
{
count++;
}
bits >>= 1;
}
return count;
}
typedef BOOL(WINAPI* Pfn_GetLogicalProcessorInformation)(PSYSTEM_LOGICAL_PROCESSOR_INFORMATION, PDWORD);
typedef BOOL( WINAPI *Pfn_GetLogicalProcessorInformation )( PSYSTEM_LOGICAL_PROCESSOR_INFORMATION, PDWORD );
void getProcessorInformation( btProcessorInfo* procInfo )
void getProcessorInformation(btProcessorInfo* procInfo)
{
memset( procInfo, 0, sizeof( *procInfo ) );
Pfn_GetLogicalProcessorInformation getLogicalProcInfo =
(Pfn_GetLogicalProcessorInformation) GetProcAddress( GetModuleHandle( TEXT( "kernel32" ) ), "GetLogicalProcessorInformation" );
if ( getLogicalProcInfo == NULL )
{
// no info
return;
}
PSYSTEM_LOGICAL_PROCESSOR_INFORMATION buf = NULL;
DWORD bufSize = 0;
while ( true )
{
if ( getLogicalProcInfo( buf, &bufSize ) )
{
break;
}
else
{
if ( GetLastError() == ERROR_INSUFFICIENT_BUFFER )
{
if ( buf )
{
free( buf );
}
buf = (PSYSTEM_LOGICAL_PROCESSOR_INFORMATION) malloc( bufSize );
}
}
}
memset(procInfo, 0, sizeof(*procInfo));
Pfn_GetLogicalProcessorInformation getLogicalProcInfo =
(Pfn_GetLogicalProcessorInformation)GetProcAddress(GetModuleHandle(TEXT("kernel32")), "GetLogicalProcessorInformation");
if (getLogicalProcInfo == NULL)
{
// no info
return;
}
PSYSTEM_LOGICAL_PROCESSOR_INFORMATION buf = NULL;
DWORD bufSize = 0;
while (true)
{
if (getLogicalProcInfo(buf, &bufSize))
{
break;
}
else
{
if (GetLastError() == ERROR_INSUFFICIENT_BUFFER)
{
if (buf)
{
free(buf);
}
buf = (PSYSTEM_LOGICAL_PROCESSOR_INFORMATION)malloc(bufSize);
}
}
}
int len = bufSize / sizeof( *buf );
for ( int i = 0; i < len; ++i )
{
PSYSTEM_LOGICAL_PROCESSOR_INFORMATION info = buf + i;
switch ( info->Relationship )
{
case RelationNumaNode:
procInfo->numNumaNodes++;
break;
int len = bufSize / sizeof(*buf);
for (int i = 0; i < len; ++i)
{
PSYSTEM_LOGICAL_PROCESSOR_INFORMATION info = buf + i;
switch (info->Relationship)
{
case RelationNumaNode:
procInfo->numNumaNodes++;
break;
case RelationProcessorCore:
procInfo->numCores++;
procInfo->numLogicalProcessors += countSetBits( info->ProcessorMask );
break;
case RelationProcessorCore:
procInfo->numCores++;
procInfo->numLogicalProcessors += countSetBits(info->ProcessorMask);
break;
case RelationCache:
if ( info->Cache.Level == 1 )
{
procInfo->numL1Cache++;
}
else if ( info->Cache.Level == 2 )
{
procInfo->numL2Cache++;
}
else if ( info->Cache.Level == 3 )
{
procInfo->numL3Cache++;
// processors that share L3 cache are considered to be on the same team
// because they can more easily work together on the same data.
// Large performance penalties will occur if 2 or more threads from different
// teams attempt to frequently read and modify the same cache lines.
//
// On the AMD Ryzen 7 CPU for example, the 8 cores on the CPU are split into
// 2 CCX units of 4 cores each. Each CCX has a separate L3 cache, so if both
// CCXs are operating on the same data, many cycles will be spent keeping the
// two caches coherent.
if ( procInfo->numTeamMasks < btProcessorInfo::maxNumTeamMasks )
{
procInfo->processorTeamMasks[ procInfo->numTeamMasks ] = info->ProcessorMask;
procInfo->numTeamMasks++;
}
}
break;
case RelationCache:
if (info->Cache.Level == 1)
{
procInfo->numL1Cache++;
}
else if (info->Cache.Level == 2)
{
procInfo->numL2Cache++;
}
else if (info->Cache.Level == 3)
{
procInfo->numL3Cache++;
// processors that share L3 cache are considered to be on the same team
// because they can more easily work together on the same data.
// Large performance penalties will occur if 2 or more threads from different
// teams attempt to frequently read and modify the same cache lines.
//
// On the AMD Ryzen 7 CPU for example, the 8 cores on the CPU are split into
// 2 CCX units of 4 cores each. Each CCX has a separate L3 cache, so if both
// CCXs are operating on the same data, many cycles will be spent keeping the
// two caches coherent.
if (procInfo->numTeamMasks < btProcessorInfo::maxNumTeamMasks)
{
procInfo->processorTeamMasks[procInfo->numTeamMasks] = info->ProcessorMask;
procInfo->numTeamMasks++;
}
}
break;
case RelationProcessorPackage:
procInfo->numPhysicalPackages++;
break;
}
}
free( buf );
case RelationProcessorPackage:
procInfo->numPhysicalPackages++;
break;
}
}
free(buf);
}
///btThreadSupportWin32 helps to initialize/shutdown libspe2, start/stop SPU tasks and communication
class btThreadSupportWin32 : public btThreadSupportInterface
{
public:
struct btThreadStatus
{
int m_taskId;
int m_commandId;
int m_status;
struct btThreadStatus
{
int m_taskId;
int m_commandId;
int m_status;
ThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc
ThreadFunc m_userThreadFunc;
void* m_userPtr; //for taskDesc etc
void* m_threadHandle; //this one is calling 'Win32ThreadFunc'
void* m_threadHandle; //this one is calling 'Win32ThreadFunc'
void* m_eventStartHandle;
char m_eventStartHandleName[ 32 ];
void* m_eventStartHandle;
char m_eventStartHandleName[32];
void* m_eventCompleteHandle;
char m_eventCompleteHandleName[ 32 ];
};
void* m_eventCompleteHandle;
char m_eventCompleteHandleName[32];
};
private:
btAlignedObjectArray<btThreadStatus> m_activeThreadStatus;
btAlignedObjectArray<void*> m_completeHandles;
int m_numThreads;
DWORD_PTR m_startedThreadMask;
btProcessorInfo m_processorInfo;
btAlignedObjectArray<btThreadStatus> m_activeThreadStatus;
btAlignedObjectArray<void*> m_completeHandles;
int m_numThreads;
DWORD_PTR m_startedThreadMask;
btProcessorInfo m_processorInfo;
void startThreads( const ConstructionInfo& threadInfo );
void stopThreads();
int waitForResponse();
void startThreads(const ConstructionInfo& threadInfo);
void stopThreads();
int waitForResponse();
public:
btThreadSupportWin32(const ConstructionInfo& threadConstructionInfo);
virtual ~btThreadSupportWin32();
btThreadSupportWin32( const ConstructionInfo& threadConstructionInfo );
virtual ~btThreadSupportWin32();
virtual int getNumWorkerThreads() const BT_OVERRIDE { return m_numThreads; }
virtual int getCacheFriendlyNumThreads() const BT_OVERRIDE { return countSetBits(m_processorInfo.processorTeamMasks[0]); }
virtual int getLogicalToPhysicalCoreRatio() const BT_OVERRIDE { return m_processorInfo.numLogicalProcessors / m_processorInfo.numCores; }
virtual int getNumWorkerThreads() const BT_OVERRIDE { return m_numThreads; }
virtual int getCacheFriendlyNumThreads() const BT_OVERRIDE { return countSetBits(m_processorInfo.processorTeamMasks[0]); }
virtual int getLogicalToPhysicalCoreRatio() const BT_OVERRIDE { return m_processorInfo.numLogicalProcessors / m_processorInfo.numCores; }
virtual void runTask(int threadIndex, void* userData) BT_OVERRIDE;
virtual void waitForAllTasks() BT_OVERRIDE;
virtual void runTask( int threadIndex, void* userData ) BT_OVERRIDE;
virtual void waitForAllTasks() BT_OVERRIDE;
virtual btCriticalSection* createCriticalSection() BT_OVERRIDE;
virtual void deleteCriticalSection( btCriticalSection* criticalSection ) BT_OVERRIDE;
virtual btCriticalSection* createCriticalSection() BT_OVERRIDE;
virtual void deleteCriticalSection(btCriticalSection* criticalSection) BT_OVERRIDE;
};
btThreadSupportWin32::btThreadSupportWin32( const ConstructionInfo & threadConstructionInfo )
btThreadSupportWin32::btThreadSupportWin32(const ConstructionInfo& threadConstructionInfo)
{
startThreads( threadConstructionInfo );
startThreads(threadConstructionInfo);
}
btThreadSupportWin32::~btThreadSupportWin32()
{
stopThreads();
stopThreads();
}
DWORD WINAPI win32threadStartFunc( LPVOID lpParam )
DWORD WINAPI win32threadStartFunc(LPVOID lpParam)
{
btThreadSupportWin32::btThreadStatus* status = ( btThreadSupportWin32::btThreadStatus* )lpParam;
btThreadSupportWin32::btThreadStatus* status = (btThreadSupportWin32::btThreadStatus*)lpParam;
while ( 1 )
{
WaitForSingleObject( status->m_eventStartHandle, INFINITE );
void* userPtr = status->m_userPtr;
while (1)
{
WaitForSingleObject(status->m_eventStartHandle, INFINITE);
void* userPtr = status->m_userPtr;
if ( userPtr )
{
btAssert( status->m_status );
status->m_userThreadFunc( userPtr );
status->m_status = 2;
SetEvent( status->m_eventCompleteHandle );
}
else
{
//exit Thread
status->m_status = 3;
printf( "Thread with taskId %i with handle %p exiting\n", status->m_taskId, status->m_threadHandle );
SetEvent( status->m_eventCompleteHandle );
break;
}
}
printf( "Thread TERMINATED\n" );
return 0;
if (userPtr)
{
btAssert(status->m_status);
status->m_userThreadFunc(userPtr);
status->m_status = 2;
SetEvent(status->m_eventCompleteHandle);
}
else
{
//exit Thread
status->m_status = 3;
printf("Thread with taskId %i with handle %p exiting\n", status->m_taskId, status->m_threadHandle);
SetEvent(status->m_eventCompleteHandle);
break;
}
}
printf("Thread TERMINATED\n");
return 0;
}
void btThreadSupportWin32::runTask( int threadIndex, void* userData )
void btThreadSupportWin32::runTask(int threadIndex, void* userData)
{
btThreadStatus& threadStatus = m_activeThreadStatus[ threadIndex ];
btAssert( threadIndex >= 0 );
btAssert( int( threadIndex ) < m_activeThreadStatus.size() );
btThreadStatus& threadStatus = m_activeThreadStatus[threadIndex];
btAssert(threadIndex >= 0);
btAssert(int(threadIndex) < m_activeThreadStatus.size());
threadStatus.m_commandId = 1;
threadStatus.m_status = 1;
threadStatus.m_userPtr = userData;
m_startedThreadMask |= DWORD_PTR( 1 ) << threadIndex;
threadStatus.m_commandId = 1;
threadStatus.m_status = 1;
threadStatus.m_userPtr = userData;
m_startedThreadMask |= DWORD_PTR(1) << threadIndex;
///fire event to start new task
SetEvent( threadStatus.m_eventStartHandle );
///fire event to start new task
SetEvent(threadStatus.m_eventStartHandle);
}
int btThreadSupportWin32::waitForResponse()
{
btAssert( m_activeThreadStatus.size() );
btAssert(m_activeThreadStatus.size());
int last = -1;
DWORD res = WaitForMultipleObjects( m_completeHandles.size(), &m_completeHandles[ 0 ], FALSE, INFINITE );
btAssert( res != WAIT_FAILED );
last = res - WAIT_OBJECT_0;
int last = -1;
DWORD res = WaitForMultipleObjects(m_completeHandles.size(), &m_completeHandles[0], FALSE, INFINITE);
btAssert(res != WAIT_FAILED);
last = res - WAIT_OBJECT_0;
btThreadStatus& threadStatus = m_activeThreadStatus[ last ];
btAssert( threadStatus.m_threadHandle );
btAssert( threadStatus.m_eventCompleteHandle );
btThreadStatus& threadStatus = m_activeThreadStatus[last];
btAssert(threadStatus.m_threadHandle);
btAssert(threadStatus.m_eventCompleteHandle);
//WaitForSingleObject(threadStatus.m_eventCompleteHandle, INFINITE);
btAssert( threadStatus.m_status > 1 );
threadStatus.m_status = 0;
//WaitForSingleObject(threadStatus.m_eventCompleteHandle, INFINITE);
btAssert(threadStatus.m_status > 1);
threadStatus.m_status = 0;
///need to find an active spu
btAssert( last >= 0 );
m_startedThreadMask &= ~( DWORD_PTR( 1 ) << last );
///need to find an active spu
btAssert(last >= 0);
m_startedThreadMask &= ~(DWORD_PTR(1) << last);
return last;
return last;
}
void btThreadSupportWin32::waitForAllTasks()
{
while ( m_startedThreadMask )
{
waitForResponse();
}
while (m_startedThreadMask)
{
waitForResponse();
}
}
void btThreadSupportWin32::startThreads( const ConstructionInfo& threadConstructionInfo )
void btThreadSupportWin32::startThreads(const ConstructionInfo& threadConstructionInfo)
{
static int uniqueId = 0;
uniqueId++;
btProcessorInfo& procInfo = m_processorInfo;
getProcessorInformation( &procInfo );
DWORD_PTR dwProcessAffinityMask = 0;
DWORD_PTR dwSystemAffinityMask = 0;
if ( !GetProcessAffinityMask( GetCurrentProcess(), &dwProcessAffinityMask, &dwSystemAffinityMask ) )
{
dwProcessAffinityMask = 0;
}
///The number of threads should be equal to the number of available cores - 1
m_numThreads = btMin(procInfo.numLogicalProcessors, int(BT_MAX_THREAD_COUNT)) - 1; // cap to max thread count (-1 because main thread already exists)
static int uniqueId = 0;
uniqueId++;
btProcessorInfo& procInfo = m_processorInfo;
getProcessorInformation(&procInfo);
DWORD_PTR dwProcessAffinityMask = 0;
DWORD_PTR dwSystemAffinityMask = 0;
if (!GetProcessAffinityMask(GetCurrentProcess(), &dwProcessAffinityMask, &dwSystemAffinityMask))
{
dwProcessAffinityMask = 0;
}
///The number of threads should be equal to the number of available cores - 1
m_numThreads = btMin(procInfo.numLogicalProcessors, int(BT_MAX_THREAD_COUNT)) - 1; // cap to max thread count (-1 because main thread already exists)
m_activeThreadStatus.resize( m_numThreads );
m_completeHandles.resize( m_numThreads );
m_startedThreadMask = 0;
m_activeThreadStatus.resize(m_numThreads);
m_completeHandles.resize(m_numThreads);
m_startedThreadMask = 0;
// set main thread affinity
if ( DWORD_PTR mask = dwProcessAffinityMask & getProcessorTeamMask( procInfo, 0 ))
{
SetThreadAffinityMask( GetCurrentThread(), mask );
SetThreadIdealProcessor( GetCurrentThread(), 0 );
}
// set main thread affinity
if (DWORD_PTR mask = dwProcessAffinityMask & getProcessorTeamMask(procInfo, 0))
{
SetThreadAffinityMask(GetCurrentThread(), mask);
SetThreadIdealProcessor(GetCurrentThread(), 0);
}
for ( int i = 0; i < m_numThreads; i++ )
{
printf( "starting thread %d\n", i );
for (int i = 0; i < m_numThreads; i++)
{
printf("starting thread %d\n", i);
btThreadStatus& threadStatus = m_activeThreadStatus[ i ];
btThreadStatus& threadStatus = m_activeThreadStatus[i];
LPSECURITY_ATTRIBUTES lpThreadAttributes = NULL;
SIZE_T dwStackSize = threadConstructionInfo.m_threadStackSize;
LPTHREAD_START_ROUTINE lpStartAddress = &win32threadStartFunc;
LPVOID lpParameter = &threadStatus;
DWORD dwCreationFlags = 0;
LPDWORD lpThreadId = 0;
LPSECURITY_ATTRIBUTES lpThreadAttributes = NULL;
SIZE_T dwStackSize = threadConstructionInfo.m_threadStackSize;
LPTHREAD_START_ROUTINE lpStartAddress = &win32threadStartFunc;
LPVOID lpParameter = &threadStatus;
DWORD dwCreationFlags = 0;
LPDWORD lpThreadId = 0;
threadStatus.m_userPtr = 0;
threadStatus.m_userPtr = 0;
sprintf( threadStatus.m_eventStartHandleName, "es%.8s%d%d", threadConstructionInfo.m_uniqueName, uniqueId, i );
threadStatus.m_eventStartHandle = CreateEventA( 0, false, false, threadStatus.m_eventStartHandleName );
sprintf(threadStatus.m_eventStartHandleName, "es%.8s%d%d", threadConstructionInfo.m_uniqueName, uniqueId, i);
threadStatus.m_eventStartHandle = CreateEventA(0, false, false, threadStatus.m_eventStartHandleName);
sprintf( threadStatus.m_eventCompleteHandleName, "ec%.8s%d%d", threadConstructionInfo.m_uniqueName, uniqueId, i );
threadStatus.m_eventCompleteHandle = CreateEventA( 0, false, false, threadStatus.m_eventCompleteHandleName );
sprintf(threadStatus.m_eventCompleteHandleName, "ec%.8s%d%d", threadConstructionInfo.m_uniqueName, uniqueId, i);
threadStatus.m_eventCompleteHandle = CreateEventA(0, false, false, threadStatus.m_eventCompleteHandleName);
m_completeHandles[ i ] = threadStatus.m_eventCompleteHandle;
m_completeHandles[i] = threadStatus.m_eventCompleteHandle;
HANDLE handle = CreateThread( lpThreadAttributes, dwStackSize, lpStartAddress, lpParameter, dwCreationFlags, lpThreadId );
//SetThreadPriority( handle, THREAD_PRIORITY_HIGHEST );
// highest priority -- can cause erratic performance when numThreads > numCores
// we don't want worker threads to be higher priority than the main thread or the main thread could get
// totally shut out and unable to tell the workers to stop
//SetThreadPriority( handle, THREAD_PRIORITY_BELOW_NORMAL );
HANDLE handle = CreateThread(lpThreadAttributes, dwStackSize, lpStartAddress, lpParameter, dwCreationFlags, lpThreadId);
//SetThreadPriority( handle, THREAD_PRIORITY_HIGHEST );
// highest priority -- can cause erratic performance when numThreads > numCores
// we don't want worker threads to be higher priority than the main thread or the main thread could get
// totally shut out and unable to tell the workers to stop
//SetThreadPriority( handle, THREAD_PRIORITY_BELOW_NORMAL );
{
int processorId = i + 1; // leave processor 0 for main thread
DWORD_PTR teamMask = getProcessorTeamMask( procInfo, processorId );
if ( teamMask )
{
// bind each thread to only execute on processors of it's assigned team
// - for single-socket Intel x86 CPUs this has no effect (only a single, shared L3 cache so there is only 1 team)
// - for multi-socket Intel this will keep threads from migrating from one socket to another
// - for AMD Ryzen this will keep threads from migrating from one CCX to another
DWORD_PTR mask = teamMask & dwProcessAffinityMask;
if ( mask )
{
SetThreadAffinityMask( handle, mask );
}
}
SetThreadIdealProcessor( handle, processorId );
}
{
int processorId = i + 1; // leave processor 0 for main thread
DWORD_PTR teamMask = getProcessorTeamMask(procInfo, processorId);
if (teamMask)
{
// bind each thread to only execute on processors of it's assigned team
// - for single-socket Intel x86 CPUs this has no effect (only a single, shared L3 cache so there is only 1 team)
// - for multi-socket Intel this will keep threads from migrating from one socket to another
// - for AMD Ryzen this will keep threads from migrating from one CCX to another
DWORD_PTR mask = teamMask & dwProcessAffinityMask;
if (mask)
{
SetThreadAffinityMask(handle, mask);
}
}
SetThreadIdealProcessor(handle, processorId);
}
threadStatus.m_taskId = i;
threadStatus.m_commandId = 0;
threadStatus.m_status = 0;
threadStatus.m_threadHandle = handle;
threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
threadStatus.m_taskId = i;
threadStatus.m_commandId = 0;
threadStatus.m_status = 0;
threadStatus.m_threadHandle = handle;
threadStatus.m_userThreadFunc = threadConstructionInfo.m_userThreadFunc;
printf( "started %s thread %d with threadHandle %p\n", threadConstructionInfo.m_uniqueName, i, handle );
}
printf("started %s thread %d with threadHandle %p\n", threadConstructionInfo.m_uniqueName, i, handle);
}
}
///tell the task scheduler we are done with the SPU tasks
void btThreadSupportWin32::stopThreads()
{
for ( int i = 0; i < m_activeThreadStatus.size(); i++ )
{
btThreadStatus& threadStatus = m_activeThreadStatus[ i ];
if ( threadStatus.m_status > 0 )
{
WaitForSingleObject( threadStatus.m_eventCompleteHandle, INFINITE );
}
for (int i = 0; i < m_activeThreadStatus.size(); i++)
{
btThreadStatus& threadStatus = m_activeThreadStatus[i];
if (threadStatus.m_status > 0)
{
WaitForSingleObject(threadStatus.m_eventCompleteHandle, INFINITE);
}
threadStatus.m_userPtr = NULL;
SetEvent( threadStatus.m_eventStartHandle );
WaitForSingleObject( threadStatus.m_eventCompleteHandle, INFINITE );
threadStatus.m_userPtr = NULL;
SetEvent(threadStatus.m_eventStartHandle);
WaitForSingleObject(threadStatus.m_eventCompleteHandle, INFINITE);
CloseHandle( threadStatus.m_eventCompleteHandle );
CloseHandle( threadStatus.m_eventStartHandle );
CloseHandle( threadStatus.m_threadHandle );
CloseHandle(threadStatus.m_eventCompleteHandle);
CloseHandle(threadStatus.m_eventStartHandle);
CloseHandle(threadStatus.m_threadHandle);
}
}
m_activeThreadStatus.clear();
m_completeHandles.clear();
m_activeThreadStatus.clear();
m_completeHandles.clear();
}
class btWin32CriticalSection : public btCriticalSection
{
private:
CRITICAL_SECTION mCriticalSection;
CRITICAL_SECTION mCriticalSection;
public:
btWin32CriticalSection()
{
InitializeCriticalSection( &mCriticalSection );
}
btWin32CriticalSection()
{
InitializeCriticalSection(&mCriticalSection);
}
~btWin32CriticalSection()
{
DeleteCriticalSection( &mCriticalSection );
}
~btWin32CriticalSection()
{
DeleteCriticalSection(&mCriticalSection);
}
void lock()
{
EnterCriticalSection( &mCriticalSection );
}
void lock()
{
EnterCriticalSection(&mCriticalSection);
}
void unlock()
{
LeaveCriticalSection( &mCriticalSection );
}
void unlock()
{
LeaveCriticalSection(&mCriticalSection);
}
};
btCriticalSection* btThreadSupportWin32::createCriticalSection()
{
unsigned char* mem = (unsigned char*) btAlignedAlloc( sizeof( btWin32CriticalSection ), 16 );
btWin32CriticalSection* cs = new( mem ) btWin32CriticalSection();
return cs;
unsigned char* mem = (unsigned char*)btAlignedAlloc(sizeof(btWin32CriticalSection), 16);
btWin32CriticalSection* cs = new (mem) btWin32CriticalSection();
return cs;
}
void btThreadSupportWin32::deleteCriticalSection( btCriticalSection* criticalSection )
void btThreadSupportWin32::deleteCriticalSection(btCriticalSection* criticalSection)
{
criticalSection->~btCriticalSection();
btAlignedFree( criticalSection );
criticalSection->~btCriticalSection();
btAlignedFree(criticalSection);
}
btThreadSupportInterface* btThreadSupportInterface::create( const ConstructionInfo& info )
btThreadSupportInterface* btThreadSupportInterface::create(const ConstructionInfo& info)
{
return new btThreadSupportWin32( info );
return new btThreadSupportWin32(info);
}
#endif //defined(_WIN32) && BT_THREADSAFE
#endif //defined(_WIN32) && BT_THREADSAFE

173
src/LinearMath/btAabbUtil2.h
View File

@@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_AABB_UTIL2
#define BT_AABB_UTIL2
@@ -21,20 +19,18 @@ subject to the following restrictions:
#include "btVector3.h"
#include "btMinMax.h"
SIMD_FORCE_INLINE void AabbExpand (btVector3& aabbMin,
btVector3& aabbMax,
const btVector3& expansionMin,
const btVector3& expansionMax)
SIMD_FORCE_INLINE void AabbExpand(btVector3& aabbMin,
btVector3& aabbMax,
const btVector3& expansionMin,
const btVector3& expansionMax)
{
aabbMin = aabbMin + expansionMin;
aabbMax = aabbMax + expansionMax;
}
/// conservative test for overlap between two aabbs
SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
const btVector3 &point)
SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3& aabbMin1, const btVector3& aabbMax1,
const btVector3& point)
{
bool overlap = true;
overlap = (aabbMin1.getX() > point.getX() || aabbMax1.getX() < point.getX()) ? false : overlap;
@@ -43,10 +39,9 @@ SIMD_FORCE_INLINE bool TestPointAgainstAabb2(const btVector3 &aabbMin1, const bt
return overlap;
}
/// conservative test for overlap between two aabbs
SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
const btVector3 &aabbMin2, const btVector3 &aabbMax2)
SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3& aabbMin1, const btVector3& aabbMax1,
const btVector3& aabbMin2, const btVector3& aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
@@ -56,37 +51,34 @@ SIMD_FORCE_INLINE bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btV
}
/// conservative test for overlap between triangle and aabb
SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3 *vertices,
const btVector3 &aabbMin, const btVector3 &aabbMax)
SIMD_FORCE_INLINE bool TestTriangleAgainstAabb2(const btVector3* vertices,
const btVector3& aabbMin, const btVector3& aabbMax)
{
const btVector3 &p1 = vertices[0];
const btVector3 &p2 = vertices[1];
const btVector3 &p3 = vertices[2];
const btVector3& p1 = vertices[0];
const btVector3& p2 = vertices[1];
const btVector3& p3 = vertices[2];
if (btMin(btMin(p1[0], p2[0]), p3[0]) > aabbMax[0]) return false;
if (btMax(btMax(p1[0], p2[0]), p3[0]) < aabbMin[0]) return false;
if (btMin(btMin(p1[2], p2[2]), p3[2]) > aabbMax[2]) return false;
if (btMax(btMax(p1[2], p2[2]), p3[2]) < aabbMin[2]) return false;
if (btMin(btMin(p1[1], p2[1]), p3[1]) > aabbMax[1]) return false;
if (btMax(btMax(p1[1], p2[1]), p3[1]) < aabbMin[1]) return false;
return true;
}
SIMD_FORCE_INLINE int btOutcode(const btVector3& p,const btVector3& halfExtent)
SIMD_FORCE_INLINE int btOutcode(const btVector3& p, const btVector3& halfExtent)
{
return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
(p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
(p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
(p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
(p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
(p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
return (p.getX() < -halfExtent.getX() ? 0x01 : 0x0) |
(p.getX() > halfExtent.getX() ? 0x08 : 0x0) |
(p.getY() < -halfExtent.getY() ? 0x02 : 0x0) |
(p.getY() > halfExtent.getY() ? 0x10 : 0x0) |
(p.getZ() < -halfExtent.getZ() ? 0x4 : 0x0) |
(p.getZ() > halfExtent.getZ() ? 0x20 : 0x0);
}
SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
const btVector3& rayInvDirection,
const unsigned int raySign[3],
@@ -97,11 +89,11 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
{
btScalar tmax, tymin, tymax, tzmin, tzmax;
tmin = (bounds[raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tmax = (bounds[1-raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tmax = (bounds[1 - raySign[0]].getX() - rayFrom.getX()) * rayInvDirection.getX();
tymin = (bounds[raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
tymax = (bounds[1-raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
tymax = (bounds[1 - raySign[1]].getY() - rayFrom.getY()) * rayInvDirection.getY();
if ( (tmin > tymax) || (tymin > tmax) )
if ((tmin > tymax) || (tymin > tmax))
return false;
if (tymin > tmin)
@@ -111,59 +103,59 @@ SIMD_FORCE_INLINE bool btRayAabb2(const btVector3& rayFrom,
tmax = tymax;
tzmin = (bounds[raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
tzmax = (bounds[1-raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
tzmax = (bounds[1 - raySign[2]].getZ() - rayFrom.getZ()) * rayInvDirection.getZ();
if ( (tmin > tzmax) || (tzmin > tmax) )
if ((tmin > tzmax) || (tzmin > tmax))
return false;
if (tzmin > tmin)
tmin = tzmin;
if (tzmax < tmax)
tmax = tzmax;
return ( (tmin < lambda_max) && (tmax > lambda_min) );
return ((tmin < lambda_max) && (tmax > lambda_min));
}
SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
const btVector3& rayTo,
const btVector3& aabbMin,
SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
const btVector3& rayTo,
const btVector3& aabbMin,
const btVector3& aabbMax,
btScalar& param, btVector3& normal)
btScalar& param, btVector3& normal)
{
btVector3 aabbHalfExtent = (aabbMax-aabbMin)* btScalar(0.5);
btVector3 aabbCenter = (aabbMax+aabbMin)* btScalar(0.5);
btVector3 source = rayFrom - aabbCenter;
btVector3 target = rayTo - aabbCenter;
int sourceOutcode = btOutcode(source,aabbHalfExtent);
int targetOutcode = btOutcode(target,aabbHalfExtent);
btVector3 aabbHalfExtent = (aabbMax - aabbMin) * btScalar(0.5);
btVector3 aabbCenter = (aabbMax + aabbMin) * btScalar(0.5);
btVector3 source = rayFrom - aabbCenter;
btVector3 target = rayTo - aabbCenter;
int sourceOutcode = btOutcode(source, aabbHalfExtent);
int targetOutcode = btOutcode(target, aabbHalfExtent);
if ((sourceOutcode & targetOutcode) == 0x0)
{
btScalar lambda_enter = btScalar(0.0);
btScalar lambda_exit = param;
btScalar lambda_exit = param;
btVector3 r = target - source;
int i;
btScalar normSign = 1;
btVector3 hitNormal(0,0,0);
int bit=1;
btScalar normSign = 1;
btVector3 hitNormal(0, 0, 0);
int bit = 1;
for (int j=0;j<2;j++)
for (int j = 0; j < 2; j++)
{
for (i = 0; i != 3; ++i)
{
if (sourceOutcode & bit)
{
btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
btScalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
if (lambda_enter <= lambda)
{
lambda_enter = lambda;
hitNormal.setValue(0,0,0);
hitNormal.setValue(0, 0, 0);
hitNormal[i] = normSign;
}
}
else if (targetOutcode & bit)
else if (targetOutcode & bit)
{
btScalar lambda = (-source[i] - aabbHalfExtent[i]*normSign) / r[i];
btScalar lambda = (-source[i] - aabbHalfExtent[i] * normSign) / r[i];
btSetMin(lambda_exit, lambda);
}
bit<<=1;
bit <<= 1;
}
normSign = btScalar(-1.);
}
@@ -177,56 +169,49 @@ SIMD_FORCE_INLINE bool btRayAabb(const btVector3& rayFrom,
return false;
}
SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin,const btTransform& t,btVector3& aabbMinOut,btVector3& aabbMaxOut)
SIMD_FORCE_INLINE void btTransformAabb(const btVector3& halfExtents, btScalar margin, const btTransform& t, btVector3& aabbMinOut, btVector3& aabbMaxOut)
{
btVector3 halfExtentsWithMargin = halfExtents+btVector3(margin,margin,margin);
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 halfExtentsWithMargin = halfExtents + btVector3(margin, margin, margin);
btMatrix3x3 abs_b = t.getBasis().absolute();
btVector3 center = t.getOrigin();
btVector3 extent = halfExtentsWithMargin.dot3( abs_b[0], abs_b[1], abs_b[2] );
btVector3 extent = halfExtentsWithMargin.dot3(abs_b[0], abs_b[1], abs_b[2]);
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin,const btVector3& localAabbMax, btScalar margin,const btTransform& trans,btVector3& aabbMinOut,btVector3& aabbMaxOut)
SIMD_FORCE_INLINE void btTransformAabb(const btVector3& localAabbMin, const btVector3& localAabbMax, btScalar margin, const btTransform& trans, btVector3& aabbMinOut, btVector3& aabbMaxOut)
{
btAssert(localAabbMin.getX() <= localAabbMax.getX());
btAssert(localAabbMin.getY() <= localAabbMax.getY());
btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
btVector3 localHalfExtents = btScalar(0.5)*(localAabbMax-localAabbMin);
localHalfExtents+=btVector3(margin,margin,margin);
btAssert(localAabbMin.getX() <= localAabbMax.getX());
btAssert(localAabbMin.getY() <= localAabbMax.getY());
btAssert(localAabbMin.getZ() <= localAabbMax.getZ());
btVector3 localHalfExtents = btScalar(0.5) * (localAabbMax - localAabbMin);
localHalfExtents += btVector3(margin, margin, margin);
btVector3 localCenter = btScalar(0.5)*(localAabbMax+localAabbMin);
btMatrix3x3 abs_b = trans.getBasis().absolute();
btVector3 center = trans(localCenter);
btVector3 extent = localHalfExtents.dot3( abs_b[0], abs_b[1], abs_b[2] );
aabbMinOut = center-extent;
aabbMaxOut = center+extent;
btVector3 localCenter = btScalar(0.5) * (localAabbMax + localAabbMin);
btMatrix3x3 abs_b = trans.getBasis().absolute();
btVector3 center = trans(localCenter);
btVector3 extent = localHalfExtents.dot3(abs_b[0], abs_b[1], abs_b[2]);
aabbMinOut = center - extent;
aabbMaxOut = center + extent;
}
#define USE_BANCHLESS 1
#ifdef USE_BANCHLESS
//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
{
return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0])
& (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2])
& (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
1, 0));
}
//This block replaces the block below and uses no branches, and replaces the 8 bit return with a 32 bit return for improved performance (~3x on XBox 360)
SIMD_FORCE_INLINE unsigned testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
{
return static_cast<unsigned int>(btSelect((unsigned)((aabbMin1[0] <= aabbMax2[0]) & (aabbMax1[0] >= aabbMin2[0]) & (aabbMin1[2] <= aabbMax2[2]) & (aabbMax1[2] >= aabbMin2[2]) & (aabbMin1[1] <= aabbMax2[1]) & (aabbMax1[1] >= aabbMin2[1])),
1, 0));
}
#else
SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1,const unsigned short int* aabbMax1,const unsigned short int* aabbMin2,const unsigned short int* aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
return overlap;
}
#endif //USE_BANCHLESS
#endif //BT_AABB_UTIL2
SIMD_FORCE_INLINE bool testQuantizedAabbAgainstQuantizedAabb(const unsigned short int* aabbMin1, const unsigned short int* aabbMax1, const unsigned short int* aabbMin2, const unsigned short int* aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1[0] > aabbMax2[0] || aabbMax1[0] < aabbMin2[0]) ? false : overlap;
overlap = (aabbMin1[2] > aabbMax2[2] || aabbMax1[2] < aabbMin2[2]) ? false : overlap;
overlap = (aabbMin1[1] > aabbMax2[1] || aabbMax1[1] < aabbMin2[1]) ? false : overlap;
return overlap;
}
#endif //USE_BANCHLESS
#endif //BT_AABB_UTIL2

246
src/LinearMath/btAlignedAllocator.cpp
View File

@@ -18,8 +18,8 @@ subject to the following restrictions:
#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
int gNumAlignedAllocs = 0;
int gNumAlignedFree = 0;
int gTotalBytesAlignedAllocs = 0;//detect memory leaks
#endif //BT_DEBUG_MEMORY_ALLOCATIONST_DEBUG_ALLOCATIONS
int gTotalBytesAlignedAllocs = 0; //detect memory leaks
#endif //BT_DEBUG_MEMORY_ALLOCATIONST_DEBUG_ALLOCATIONS
static void *btAllocDefault(size_t size)
{
@@ -34,9 +34,7 @@ static void btFreeDefault(void *ptr)
static btAllocFunc *sAllocFunc = btAllocDefault;
static btFreeFunc *sFreeFunc = btFreeDefault;
#if defined (BT_HAS_ALIGNED_ALLOCATOR)
#if defined(BT_HAS_ALIGNED_ALLOCATOR)
#include <malloc.h>
static void *btAlignedAllocDefault(size_t size, int alignment)
{
@@ -61,49 +59,48 @@ static inline void btAlignedFreeDefault(void *ptr)
}
#else
static inline void *btAlignedAllocDefault(size_t size, int alignment)
{
void *ret;
char *real;
real = (char *)sAllocFunc(size + sizeof(void *) + (alignment-1));
if (real) {
ret = btAlignPointer(real + sizeof(void *),alignment);
*((void **)(ret)-1) = (void *)(real);
} else {
ret = (void *)(real);
}
return (ret);
void *ret;
char *real;
real = (char *)sAllocFunc(size + sizeof(void *) + (alignment - 1));
if (real)
{
ret = btAlignPointer(real + sizeof(void *), alignment);
*((void **)(ret)-1) = (void *)(real);
}
else
{
ret = (void *)(real);
}
return (ret);
}
static inline void btAlignedFreeDefault(void *ptr)
{
void* real;
void *real;
if (ptr) {
real = *((void **)(ptr)-1);
sFreeFunc(real);
}
if (ptr)
{
real = *((void **)(ptr)-1);
sFreeFunc(real);
}
}
#endif
static btAlignedAllocFunc *sAlignedAllocFunc = btAlignedAllocDefault;
static btAlignedFreeFunc *sAlignedFreeFunc = btAlignedFreeDefault;
void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc)
{
sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault;
sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault;
sAlignedAllocFunc = allocFunc ? allocFunc : btAlignedAllocDefault;
sAlignedFreeFunc = freeFunc ? freeFunc : btAlignedFreeDefault;
}
void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc)
{
sAllocFunc = allocFunc ? allocFunc : btAllocDefault;
sFreeFunc = freeFunc ? freeFunc : btFreeDefault;
sAllocFunc = allocFunc ? allocFunc : btAllocDefault;
sFreeFunc = freeFunc ? freeFunc : btFreeDefault;
}
#ifdef BT_DEBUG_MEMORY_ALLOCATIONS
@@ -116,15 +113,15 @@ static int mynumallocs = 0;
int btDumpMemoryLeaks()
{
int totalLeak = 0;
for (int i=0;i<mynumallocs;i++)
for (int i = 0; i < mynumallocs; i++)
{
printf("Error: leaked memory of allocation #%d (%d bytes)\n", allocations_id[i], allocations_bytes[i]);
totalLeak+=allocations_bytes[i];
totalLeak += allocations_bytes[i];
}
if (totalLeak)
{
printf("Error: memory leaks: %d allocations were not freed and leaked together %d bytes\n",mynumallocs,totalLeak);
printf("Error: memory leaks: %d allocations were not freed and leaked together %d bytes\n", mynumallocs, totalLeak);
}
return totalLeak;
}
@@ -133,137 +130,134 @@ int btDumpMemoryLeaks()
struct btDebugPtrMagic
{
union
{
void** vptrptr;
void* vptr;
int* iptr;
char* cptr;
union {
void **vptrptr;
void *vptr;
int *iptr;
char *cptr;
};
};
void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename)
void *btAlignedAllocInternal(size_t size, int alignment, int line, char *filename)
{
if (size==0)
if (size == 0)
{
printf("Whaat? size==0");
return 0;
}
static int allocId = 0;
void *ret;
char *real;
// to find some particular memory leak, you could do something like this:
// if (allocId==172)
// {
// printf("catch me!\n");
// }
// if (size>1024*1024)
// {
// printf("big alloc!%d\n", size);
// }
void *ret;
char *real;
gTotalBytesAlignedAllocs += size;
gNumAlignedAllocs++;
// to find some particular memory leak, you could do something like this:
// if (allocId==172)
// {
// printf("catch me!\n");
// }
// if (size>1024*1024)
// {
// printf("big alloc!%d\n", size);
// }
int sz4prt = 4*sizeof(void *);
real = (char *)sAllocFunc(size + sz4prt + (alignment-1));
if (real) {
ret = (void*) btAlignPointer(real + sz4prt, alignment);
btDebugPtrMagic p;
p.vptr = ret;
p.cptr-=sizeof(void*);
*p.vptrptr = (void*)real;
p.cptr-=sizeof(void*);
*p.iptr = size;
p.cptr-=sizeof(void*);
*p.iptr = allocId;
allocations_id[mynumallocs] = allocId;
allocations_bytes[mynumallocs] = size;
mynumallocs++;
gTotalBytesAlignedAllocs += size;
gNumAlignedAllocs++;
} else {
ret = (void *)(real);//??
}
int sz4prt = 4 * sizeof(void *);
printf("allocation %d at address %x, from %s,line %d, size %d (total allocated = %d)\n",allocId,real, filename,line,size,gTotalBytesAlignedAllocs);
real = (char *)sAllocFunc(size + sz4prt + (alignment - 1));
if (real)
{
ret = (void *)btAlignPointer(real + sz4prt, alignment);
btDebugPtrMagic p;
p.vptr = ret;
p.cptr -= sizeof(void *);
*p.vptrptr = (void *)real;
p.cptr -= sizeof(void *);
*p.iptr = size;
p.cptr -= sizeof(void *);
*p.iptr = allocId;
allocations_id[mynumallocs] = allocId;
allocations_bytes[mynumallocs] = size;
mynumallocs++;
}
else
{
ret = (void *)(real); //??
}
printf("allocation %d at address %x, from %s,line %d, size %d (total allocated = %d)\n", allocId, real, filename, line, size, gTotalBytesAlignedAllocs);
allocId++;
int* ptr = (int*)ret;
*ptr = 12;
return (ret);
int *ptr = (int *)ret;
*ptr = 12;
return (ret);
}
void btAlignedFreeInternal (void* ptr,int line,char* filename)
void btAlignedFreeInternal(void *ptr, int line, char *filename)
{
void *real;
void* real;
if (ptr)
{
gNumAlignedFree++;
if (ptr) {
gNumAlignedFree++;
btDebugPtrMagic p;
p.vptr = ptr;
p.cptr -= sizeof(void *);
real = *p.vptrptr;
p.cptr -= sizeof(void *);
int size = *p.iptr;
p.cptr -= sizeof(void *);
int allocId = *p.iptr;
btDebugPtrMagic p;
p.vptr = ptr;
p.cptr-=sizeof(void*);
real = *p.vptrptr;
p.cptr-=sizeof(void*);
int size = *p.iptr;
p.cptr-=sizeof(void*);
int allocId = *p.iptr;
bool found = false;
bool found = false;
for (int i=0;i<mynumallocs;i++)
{
if ( allocations_id[i] == allocId)
{
allocations_id[i] = allocations_id[mynumallocs-1];
allocations_bytes[i] = allocations_bytes[mynumallocs-1];
mynumallocs--;
found = true;
break;
}
}
gTotalBytesAlignedAllocs -= size;
for (int i = 0; i < mynumallocs; i++)
{
if (allocations_id[i] == allocId)
{
allocations_id[i] = allocations_id[mynumallocs - 1];
allocations_bytes[i] = allocations_bytes[mynumallocs - 1];
mynumallocs--;
found = true;
break;
}
}
int diff = gNumAlignedAllocs-gNumAlignedFree;
printf("free %d at address %x, from %s,line %d, size %d (total remain = %d in %d non-freed allocations)\n",allocId,real, filename,line,size, gTotalBytesAlignedAllocs, diff);
gTotalBytesAlignedAllocs -= size;
sFreeFunc(real);
} else
{
//printf("deleting a NULL ptr, no effect\n");
}
int diff = gNumAlignedAllocs - gNumAlignedFree;
printf("free %d at address %x, from %s,line %d, size %d (total remain = %d in %d non-freed allocations)\n", allocId, real, filename, line, size, gTotalBytesAlignedAllocs, diff);
sFreeFunc(real);
}
else
{
//printf("deleting a NULL ptr, no effect\n");
}
}
#else //BT_DEBUG_MEMORY_ALLOCATIONS
#else //BT_DEBUG_MEMORY_ALLOCATIONS
void* btAlignedAllocInternal (size_t size, int alignment)
void *btAlignedAllocInternal(size_t size, int alignment)
{
void* ptr;
void *ptr;
ptr = sAlignedAllocFunc(size, alignment);
// printf("btAlignedAllocInternal %d, %x\n",size,ptr);
// printf("btAlignedAllocInternal %d, %x\n",size,ptr);
return ptr;
}
void btAlignedFreeInternal (void* ptr)
void btAlignedFreeInternal(void *ptr)
{
if (!ptr)
{
return;
}
// printf("btAlignedFreeInternal %x\n",ptr);
// printf("btAlignedFreeInternal %x\n",ptr);
sAlignedFreeFunc(ptr);
}
#endif //BT_DEBUG_MEMORY_ALLOCATIONS
#endif //BT_DEBUG_MEMORY_ALLOCATIONS

100
src/LinearMath/btAlignedAllocator.h
View File

@@ -22,7 +22,6 @@ subject to the following restrictions:
#include "btScalar.h"
///BT_DEBUG_MEMORY_ALLOCATIONS preprocessor can be set in build system
///for regression tests to detect memory leaks
///#define BT_DEBUG_MEMORY_ALLOCATIONS 1
@@ -30,84 +29,87 @@ subject to the following restrictions:
int btDumpMemoryLeaks();
#define btAlignedAlloc(a,b) \
btAlignedAllocInternal(a,b,__LINE__,__FILE__)
#define btAlignedAlloc(a, b) \
btAlignedAllocInternal(a, b, __LINE__, __FILE__)
#define btAlignedFree(ptr) \
btAlignedFreeInternal(ptr,__LINE__,__FILE__)
btAlignedFreeInternal(ptr, __LINE__, __FILE__)
void* btAlignedAllocInternal (size_t size, int alignment,int line,char* filename);
void* btAlignedAllocInternal(size_t size, int alignment, int line, char* filename);
void btAlignedFreeInternal (void* ptr,int line,char* filename);
void btAlignedFreeInternal(void* ptr, int line, char* filename);
#else
void* btAlignedAllocInternal (size_t size, int alignment);
void btAlignedFreeInternal (void* ptr);
void* btAlignedAllocInternal(size_t size, int alignment);
void btAlignedFreeInternal(void* ptr);
#define btAlignedAlloc(size,alignment) btAlignedAllocInternal(size,alignment)
#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
#define btAlignedAlloc(size, alignment) btAlignedAllocInternal(size, alignment)
#define btAlignedFree(ptr) btAlignedFreeInternal(ptr)
#endif
typedef int size_type;
typedef int size_type;
typedef void *(btAlignedAllocFunc)(size_t size, int alignment);
typedef void (btAlignedFreeFunc)(void *memblock);
typedef void *(btAllocFunc)(size_t size);
typedef void (btFreeFunc)(void *memblock);
typedef void*(btAlignedAllocFunc)(size_t size, int alignment);
typedef void(btAlignedFreeFunc)(void* memblock);
typedef void*(btAllocFunc)(size_t size);
typedef void(btFreeFunc)(void* memblock);
///The developer can let all Bullet memory allocations go through a custom memory allocator, using btAlignedAllocSetCustom
void btAlignedAllocSetCustom(btAllocFunc *allocFunc, btFreeFunc *freeFunc);
void btAlignedAllocSetCustom(btAllocFunc* allocFunc, btFreeFunc* freeFunc);
///If the developer has already an custom aligned allocator, then btAlignedAllocSetCustomAligned can be used. The default aligned allocator pre-allocates extra memory using the non-aligned allocator, and instruments it.
void btAlignedAllocSetCustomAligned(btAlignedAllocFunc *allocFunc, btAlignedFreeFunc *freeFunc);
void btAlignedAllocSetCustomAligned(btAlignedAllocFunc* allocFunc, btAlignedFreeFunc* freeFunc);
///The btAlignedAllocator is a portable class for aligned memory allocations.
///Default implementations for unaligned and aligned allocations can be overridden by a custom allocator using btAlignedAllocSetCustom and btAlignedAllocSetCustomAligned.
template < typename T , unsigned Alignment >
class btAlignedAllocator {
typedef btAlignedAllocator< T , Alignment > self_type;
public:
template <typename T, unsigned Alignment>
class btAlignedAllocator
{
typedef btAlignedAllocator<T, Alignment> self_type;
public:
//just going down a list:
btAlignedAllocator() {}
/*
btAlignedAllocator( const self_type & ) {}
*/
template < typename Other >
btAlignedAllocator( const btAlignedAllocator< Other , Alignment > & ) {}
template <typename Other>
btAlignedAllocator(const btAlignedAllocator<Other, Alignment>&)
{
}
typedef const T* const_pointer;
typedef const T& const_reference;
typedef T* pointer;
typedef T& reference;
typedef T value_type;
typedef const T* const_pointer;
typedef const T& const_reference;
typedef T* pointer;
typedef T& reference;
typedef T value_type;
pointer address ( reference ref ) const { return &ref; }
const_pointer address ( const_reference ref ) const { return &ref; }
pointer allocate ( size_type n , const_pointer * hint = 0 ) {
pointer address(reference ref) const { return &ref; }
const_pointer address(const_reference ref) const { return &ref; }
pointer allocate(size_type n, const_pointer* hint = 0)
{
(void)hint;
return reinterpret_cast< pointer >(btAlignedAlloc( sizeof(value_type) * n , Alignment ));
return reinterpret_cast<pointer>(btAlignedAlloc(sizeof(value_type) * n, Alignment));
}
void construct ( pointer ptr , const value_type & value ) { new (ptr) value_type( value ); }
void deallocate( pointer ptr ) {
btAlignedFree( reinterpret_cast< void * >( ptr ) );
void construct(pointer ptr, const value_type& value) { new (ptr) value_type(value); }
void deallocate(pointer ptr)
{
btAlignedFree(reinterpret_cast<void*>(ptr));
}
void destroy ( pointer ptr ) { ptr->~value_type(); }
void destroy(pointer ptr) { ptr->~value_type(); }
template < typename O > struct rebind {
typedef btAlignedAllocator< O , Alignment > other;
template <typename O>
struct rebind
{
typedef btAlignedAllocator<O, Alignment> other;
};
template < typename O >
self_type & operator=( const btAlignedAllocator< O , Alignment > & ) { return *this; }
template <typename O>
self_type& operator=(const btAlignedAllocator<O, Alignment>&)
{
return *this;
}
friend bool operator==( const self_type & , const self_type & ) { return true; }
friend bool operator==(const self_type&, const self_type&) { return true; }
};
#endif //BT_ALIGNED_ALLOCATOR
#endif //BT_ALIGNED_ALLOCATOR

717
src/LinearMath/btAlignedObjectArray.h
View File

@@ -13,11 +13,10 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_OBJECT_ARRAY__
#define BT_OBJECT_ARRAY__
#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
#include "btAlignedAllocator.h"
///If the platform doesn't support placement new, you can disable BT_USE_PLACEMENT_NEW
@@ -28,16 +27,16 @@ subject to the following restrictions:
#define BT_USE_PLACEMENT_NEW 1
//#define BT_USE_MEMCPY 1 //disable, because it is cumbersome to find out for each platform where memcpy is defined. It can be in <memory.h> or <string.h> or otherwise...
#define BT_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
#define BT_ALLOW_ARRAY_COPY_OPERATOR // enabling this can accidently perform deep copies of data if you are not careful
#ifdef BT_USE_MEMCPY
#include <memory.h>
#include <string.h>
#endif //BT_USE_MEMCPY
#endif //BT_USE_MEMCPY
#ifdef BT_USE_PLACEMENT_NEW
#include <new> //for placement new
#endif //BT_USE_PLACEMENT_NEW
#include <new> //for placement new
#endif //BT_USE_PLACEMENT_NEW
// The register keyword is deprecated in C++11 so don't use it.
#if __cplusplus > 199711L
@@ -48,374 +47,358 @@ subject to the following restrictions:
///The btAlignedObjectArray template class uses a subset of the stl::vector interface for its methods
///It is developed to replace stl::vector to avoid portability issues, including STL alignment issues to add SIMD/SSE data
template <typename T>
//template <class T>
template <typename T>
//template <class T>
class btAlignedObjectArray
{
btAlignedAllocator<T , 16> m_allocator;
btAlignedAllocator<T, 16> m_allocator;
int m_size;
int m_capacity;
T* m_data;
int m_size;
int m_capacity;
T* m_data;
//PCK: added this line
bool m_ownsMemory;
bool m_ownsMemory;
#ifdef BT_ALLOW_ARRAY_COPY_OPERATOR
public:
SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T> &other)
SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T>& other)
{
copyFromArray(other);
return *this;
}
#else//BT_ALLOW_ARRAY_COPY_OPERATOR
#else //BT_ALLOW_ARRAY_COPY_OPERATOR
private:
SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T> &other);
#endif//BT_ALLOW_ARRAY_COPY_OPERATOR
SIMD_FORCE_INLINE btAlignedObjectArray<T>& operator=(const btAlignedObjectArray<T>& other);
#endif //BT_ALLOW_ARRAY_COPY_OPERATOR
protected:
SIMD_FORCE_INLINE int allocSize(int size)
{
return (size ? size*2 : 1);
}
SIMD_FORCE_INLINE void copy(int start,int end, T* dest) const
{
int i;
for (i=start;i<end;++i)
SIMD_FORCE_INLINE int allocSize(int size)
{
return (size ? size * 2 : 1);
}
SIMD_FORCE_INLINE void copy(int start, int end, T* dest) const
{
int i;
for (i = start; i < end; ++i)
#ifdef BT_USE_PLACEMENT_NEW
new (&dest[i]) T(m_data[i]);
new (&dest[i]) T(m_data[i]);
#else
dest[i] = m_data[i];
#endif //BT_USE_PLACEMENT_NEW
}
dest[i] = m_data[i];
#endif //BT_USE_PLACEMENT_NEW
}
SIMD_FORCE_INLINE void init()
SIMD_FORCE_INLINE void init()
{
//PCK: added this line
m_ownsMemory = true;
m_data = 0;
m_size = 0;
m_capacity = 0;
}
SIMD_FORCE_INLINE void destroy(int first, int last)
{
int i;
for (i = first; i < last; i++)
{
//PCK: added this line
m_ownsMemory = true;
m_data = 0;
m_size = 0;
m_capacity = 0;
m_data[i].~T();
}
SIMD_FORCE_INLINE void destroy(int first,int last)
}
SIMD_FORCE_INLINE void* allocate(int size)
{
if (size)
return m_allocator.allocate(size);
return 0;
}
SIMD_FORCE_INLINE void deallocate()
{
if (m_data)
{
int i;
for (i=first; i<last;i++)
//PCK: enclosed the deallocation in this block
if (m_ownsMemory)
{
m_allocator.deallocate(m_data);
}
m_data = 0;
}
}
public:
btAlignedObjectArray()
{
init();
}
~btAlignedObjectArray()
{
clear();
}
///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
btAlignedObjectArray(const btAlignedObjectArray& otherArray)
{
init();
int otherSize = otherArray.size();
resize(otherSize);
otherArray.copy(0, otherSize, m_data);
}
/// return the number of elements in the array
SIMD_FORCE_INLINE int size() const
{
return m_size;
}
SIMD_FORCE_INLINE const T& at(int n) const
{
btAssert(n >= 0);
btAssert(n < size());
return m_data[n];
}
SIMD_FORCE_INLINE T& at(int n)
{
btAssert(n >= 0);
btAssert(n < size());
return m_data[n];
}
SIMD_FORCE_INLINE const T& operator[](int n) const
{
btAssert(n >= 0);
btAssert(n < size());
return m_data[n];
}
SIMD_FORCE_INLINE T& operator[](int n)
{
btAssert(n >= 0);
btAssert(n < size());
return m_data[n];
}
///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
SIMD_FORCE_INLINE void clear()
{
destroy(0, size());
deallocate();
init();
}
SIMD_FORCE_INLINE void pop_back()
{
btAssert(m_size > 0);
m_size--;
m_data[m_size].~T();
}
///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
{
if (newsize > size())
{
reserve(newsize);
}
m_size = newsize;
}
SIMD_FORCE_INLINE void resize(int newsize, const T& fillData = T())
{
const BT_REGISTER int curSize = size();
if (newsize < curSize)
{
for (int i = newsize; i < curSize; i++)
{
m_data[i].~T();
}
}
SIMD_FORCE_INLINE void* allocate(int size)
else
{
if (size)
return m_allocator.allocate(size);
return 0;
}
SIMD_FORCE_INLINE void deallocate()
{
if(m_data) {
//PCK: enclosed the deallocation in this block
if (m_ownsMemory)
{
m_allocator.deallocate(m_data);
}
m_data = 0;
}
}
public:
btAlignedObjectArray()
{
init();
}
~btAlignedObjectArray()
{
clear();
}
///Generally it is best to avoid using the copy constructor of an btAlignedObjectArray, and use a (const) reference to the array instead.
btAlignedObjectArray(const btAlignedObjectArray& otherArray)
{
init();
int otherSize = otherArray.size();
resize (otherSize);
otherArray.copy(0, otherSize, m_data);
}
/// return the number of elements in the array
SIMD_FORCE_INLINE int size() const
{
return m_size;
}
SIMD_FORCE_INLINE const T& at(int n) const
{
btAssert(n>=0);
btAssert(n<size());
return m_data[n];
}
SIMD_FORCE_INLINE T& at(int n)
{
btAssert(n>=0);
btAssert(n<size());
return m_data[n];
}
SIMD_FORCE_INLINE const T& operator[](int n) const
{
btAssert(n>=0);
btAssert(n<size());
return m_data[n];
}
SIMD_FORCE_INLINE T& operator[](int n)
{
btAssert(n>=0);
btAssert(n<size());
return m_data[n];
}
///clear the array, deallocated memory. Generally it is better to use array.resize(0), to reduce performance overhead of run-time memory (de)allocations.
SIMD_FORCE_INLINE void clear()
{
destroy(0,size());
deallocate();
init();
}
SIMD_FORCE_INLINE void pop_back()
{
btAssert(m_size>0);
m_size--;
m_data[m_size].~T();
}
///resize changes the number of elements in the array. If the new size is larger, the new elements will be constructed using the optional second argument.
///when the new number of elements is smaller, the destructor will be called, but memory will not be freed, to reduce performance overhead of run-time memory (de)allocations.
SIMD_FORCE_INLINE void resizeNoInitialize(int newsize)
{
if (newsize > size())
if (newsize > curSize)
{
reserve(newsize);
}
m_size = newsize;
#ifdef BT_USE_PLACEMENT_NEW
for (int i = curSize; i < newsize; i++)
{
new (&m_data[i]) T(fillData);
}
#endif //BT_USE_PLACEMENT_NEW
}
SIMD_FORCE_INLINE void resize(int newsize, const T& fillData=T())
m_size = newsize;
}
SIMD_FORCE_INLINE T& expandNonInitializing()
{
const BT_REGISTER int sz = size();
if (sz == capacity())
{
const BT_REGISTER int curSize = size();
if (newsize < curSize)
{
for(int i = newsize; i < curSize; i++)
{
m_data[i].~T();
}
} else
{
if (newsize > curSize)
{
reserve(newsize);
}
#ifdef BT_USE_PLACEMENT_NEW
for (int i=curSize;i<newsize;i++)
{
new ( &m_data[i]) T(fillData);
}
#endif //BT_USE_PLACEMENT_NEW
}
m_size = newsize;
reserve(allocSize(size()));
}
SIMD_FORCE_INLINE T& expandNonInitializing( )
{
const BT_REGISTER int sz = size();
if( sz == capacity() )
{
reserve( allocSize(size()) );
}
m_size++;
m_size++;
return m_data[sz];
return m_data[sz];
}
SIMD_FORCE_INLINE T& expand(const T& fillValue = T())
{
const BT_REGISTER int sz = size();
if (sz == capacity())
{
reserve(allocSize(size()));
}
SIMD_FORCE_INLINE T& expand( const T& fillValue=T())
{
const BT_REGISTER int sz = size();
if( sz == capacity() )
{
reserve( allocSize(size()) );
}
m_size++;
m_size++;
#ifdef BT_USE_PLACEMENT_NEW
new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
new (&m_data[sz]) T(fillValue); //use the in-place new (not really allocating heap memory)
#endif
return m_data[sz];
return m_data[sz];
}
SIMD_FORCE_INLINE void push_back(const T& _Val)
{
const BT_REGISTER int sz = size();
if (sz == capacity())
{
reserve(allocSize(size()));
}
SIMD_FORCE_INLINE void push_back(const T& _Val)
{
const BT_REGISTER int sz = size();
if( sz == capacity() )
{
reserve( allocSize(size()) );
}
#ifdef BT_USE_PLACEMENT_NEW
new ( &m_data[m_size] ) T(_Val);
new (&m_data[m_size]) T(_Val);
#else
m_data[size()] = _Val;
#endif //BT_USE_PLACEMENT_NEW
m_data[size()] = _Val;
#endif //BT_USE_PLACEMENT_NEW
m_size++;
m_size++;
}
/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
SIMD_FORCE_INLINE int capacity() const
{
return m_capacity;
}
SIMD_FORCE_INLINE void reserve(int _Count)
{ // determine new minimum length of allocated storage
if (capacity() < _Count)
{ // not enough room, reallocate
T* s = (T*)allocate(_Count);
copy(0, size(), s);
destroy(0, size());
deallocate();
//PCK: added this line
m_ownsMemory = true;
m_data = s;
m_capacity = _Count;
}
}
/// return the pre-allocated (reserved) elements, this is at least as large as the total number of elements,see size() and reserve()
SIMD_FORCE_INLINE int capacity() const
{
return m_capacity;
}
SIMD_FORCE_INLINE void reserve(int _Count)
{ // determine new minimum length of allocated storage
if (capacity() < _Count)
{ // not enough room, reallocate
T* s = (T*)allocate(_Count);
copy(0, size(), s);
destroy(0,size());
deallocate();
//PCK: added this line
m_ownsMemory = true;
m_data = s;
m_capacity = _Count;
}
}
class less
class less
{
public:
bool operator()(const T& a, const T& b) const
{
public:
return (a < b);
}
};
bool operator() ( const T& a, const T& b ) const
{
return ( a < b );
}
};
template <typename L>
void quickSortInternal(const L& CompareFunc,int lo, int hi)
{
template <typename L>
void quickSortInternal(const L& CompareFunc, int lo, int hi)
{
// lo is the lower index, hi is the upper index
// of the region of array a that is to be sorted
int i=lo, j=hi;
T x=m_data[(lo+hi)/2];
int i = lo, j = hi;
T x = m_data[(lo + hi) / 2];
// partition
do
{
while (CompareFunc(m_data[i],x))
i++;
while (CompareFunc(x,m_data[j]))
j--;
if (i<=j)
{
swap(i,j);
i++; j--;
}
} while (i<=j);
// recursion
if (lo<j)
quickSortInternal( CompareFunc, lo, j);
if (i<hi)
quickSortInternal( CompareFunc, i, hi);
}
template <typename L>
void quickSort(const L& CompareFunc)
// partition
do
{
//don't sort 0 or 1 elements
if (size()>1)
while (CompareFunc(m_data[i], x))
i++;
while (CompareFunc(x, m_data[j]))
j--;
if (i <= j)
{
quickSortInternal(CompareFunc,0,size()-1);
swap(i, j);
i++;
j--;
}
} while (i <= j);
// recursion
if (lo < j)
quickSortInternal(CompareFunc, lo, j);
if (i < hi)
quickSortInternal(CompareFunc, i, hi);
}
template <typename L>
void quickSort(const L& CompareFunc)
{
//don't sort 0 or 1 elements
if (size() > 1)
{
quickSortInternal(CompareFunc, 0, size() - 1);
}
}
///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
template <typename L>
void downHeap(T* pArr, int k, int n, const L& CompareFunc)
{
/* PRE: a[k+1..N] is a heap */
/* POST: a[k..N] is a heap */
T temp = pArr[k - 1];
/* k has child(s) */
while (k <= n / 2)
{
int child = 2 * k;
if ((child < n) && CompareFunc(pArr[child - 1], pArr[child]))
{
child++;
}
/* pick larger child */
if (CompareFunc(temp, pArr[child - 1]))
{
/* move child up */
pArr[k - 1] = pArr[child - 1];
k = child;
}
else
{
break;
}
}
pArr[k - 1] = temp;
} /*downHeap*/
///heap sort from http://www.csse.monash.edu.au/~lloyd/tildeAlgDS/Sort/Heap/
template <typename L>
void downHeap(T *pArr, int k, int n, const L& CompareFunc)
{
/* PRE: a[k+1..N] is a heap */
/* POST: a[k..N] is a heap */
T temp = pArr[k - 1];
/* k has child(s) */
while (k <= n/2)
{
int child = 2*k;
if ((child < n) && CompareFunc(pArr[child - 1] , pArr[child]))
{
child++;
}
/* pick larger child */
if (CompareFunc(temp , pArr[child - 1]))
{
/* move child up */
pArr[k - 1] = pArr[child - 1];
k = child;
}
else
{
break;
}
}
pArr[k - 1] = temp;
} /*downHeap*/
void swap(int index0,int index1)
{
void swap(int index0, int index1)
{
#ifdef BT_USE_MEMCPY
char temp[sizeof(T)];
memcpy(temp,&m_data[index0],sizeof(T));
memcpy(&m_data[index0],&m_data[index1],sizeof(T));
memcpy(&m_data[index1],temp,sizeof(T));
char temp[sizeof(T)];
memcpy(temp, &m_data[index0], sizeof(T));
memcpy(&m_data[index0], &m_data[index1], sizeof(T));
memcpy(&m_data[index1], temp, sizeof(T));
#else
T temp = m_data[index0];
m_data[index0] = m_data[index1];
m_data[index1] = temp;
#endif //BT_USE_PLACEMENT_NEW
}
T temp = m_data[index0];
m_data[index0] = m_data[index1];
m_data[index1] = temp;
#endif //BT_USE_PLACEMENT_NEW
}
template <typename L>
void heapSort(const L& CompareFunc)
@@ -423,49 +406,48 @@ protected:
/* sort a[0..N-1], N.B. 0 to N-1 */
int k;
int n = m_size;
for (k = n/2; k > 0; k--)
for (k = n / 2; k > 0; k--)
{
downHeap(m_data, k, n, CompareFunc);
}
/* a[1..N] is now a heap */
while ( n>=1 )
while (n >= 1)
{
swap(0,n-1); /* largest of a[0..n-1] */
swap(0, n - 1); /* largest of a[0..n-1] */
n = n - 1;
/* restore a[1..i-1] heap */
downHeap(m_data, 1, n, CompareFunc);
}
}
}
///non-recursive binary search, assumes sorted array
int findBinarySearch(const T& key) const
int findBinarySearch(const T& key) const
{
int first = 0;
int last = size()-1;
int last = size() - 1;
//assume sorted array
while (first <= last) {
while (first <= last)
{
int mid = (first + last) / 2; // compute mid point.
if (key > m_data[mid])
if (key > m_data[mid])
first = mid + 1; // repeat search in top half.
else if (key < m_data[mid])
last = mid - 1; // repeat search in bottom half.
else if (key < m_data[mid])
last = mid - 1; // repeat search in bottom half.
else
return mid; // found it. return position /////
return mid; // found it. return position /////
}
return size(); // failed to find key
return size(); // failed to find key
}
int findLinearSearch(const T& key) const
int findLinearSearch(const T& key) const
{
int index=size();
int index = size();
int i;
for (i=0;i<size();i++)
for (i = 0; i < size(); i++)
{
if (m_data[i] == key)
{
@@ -475,41 +457,41 @@ protected:
}
return index;
}
// If the key is not in the array, return -1 instead of 0,
// since 0 also means the first element in the array.
int findLinearSearch2(const T& key) const
{
int index=-1;
int i;
for (i=0;i<size();i++)
{
if (m_data[i] == key)
{
index = i;
break;
}
}
return index;
}
void removeAtIndex(int index)
{
if (index<size())
{
swap( index,size()-1);
pop_back();
}
}
void remove(const T& key)
// If the key is not in the array, return -1 instead of 0,
// since 0 also means the first element in the array.
int findLinearSearch2(const T& key) const
{
int index = -1;
int i;
for (i = 0; i < size(); i++)
{
if (m_data[i] == key)
{
index = i;
break;
}
}
return index;
}
void removeAtIndex(int index)
{
if (index < size())
{
swap(index, size() - 1);
pop_back();
}
}
void remove(const T& key)
{
int findIndex = findLinearSearch(key);
removeAtIndex(findIndex);
removeAtIndex(findIndex);
}
//PCK: whole function
void initializeFromBuffer(void *buffer, int size, int capacity)
void initializeFromBuffer(void* buffer, int size, int capacity)
{
clear();
m_ownsMemory = false;
@@ -521,10 +503,9 @@ protected:
void copyFromArray(const btAlignedObjectArray& otherArray)
{
int otherSize = otherArray.size();
resize (otherSize);
resize(otherSize);
otherArray.copy(0, otherSize, m_data);
}
};
#endif //BT_OBJECT_ARRAY__
#endif //BT_OBJECT_ARRAY__

947
src/LinearMath/btConvexHull.cpp
View File

File diff suppressed because it is too large Load Diff

178
src/LinearMath/btConvexHull.h
View File

@@ -34,106 +34,102 @@ public:
mNumFaces = 0;
mNumIndices = 0;
}
bool mPolygons; // true if indices represents polygons, false indices are triangles
unsigned int mNumOutputVertices; // number of vertices in the output hull
btAlignedObjectArray<btVector3> m_OutputVertices; // array of vertices
unsigned int mNumFaces; // the number of faces produced
unsigned int mNumIndices; // the total number of indices
btAlignedObjectArray<unsigned int> m_Indices; // pointer to indices.
bool mPolygons; // true if indices represents polygons, false indices are triangles
unsigned int mNumOutputVertices; // number of vertices in the output hull
btAlignedObjectArray<btVector3> m_OutputVertices; // array of vertices
unsigned int mNumFaces; // the number of faces produced
unsigned int mNumIndices; // the total number of indices
btAlignedObjectArray<unsigned int> m_Indices; // pointer to indices.
// If triangles, then indices are array indexes into the vertex list.
// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
// If triangles, then indices are array indexes into the vertex list.
// If polygons, indices are in the form (number of points in face) (p1, p2, p3, ..) etc..
};
enum HullFlag
{
QF_TRIANGLES = (1<<0), // report results as triangles, not polygons.
QF_REVERSE_ORDER = (1<<1), // reverse order of the triangle indices.
QF_DEFAULT = QF_TRIANGLES
QF_TRIANGLES = (1 << 0), // report results as triangles, not polygons.
QF_REVERSE_ORDER = (1 << 1), // reverse order of the triangle indices.
QF_DEFAULT = QF_TRIANGLES
};
class HullDesc
{
public:
HullDesc(void)
{
mFlags = QF_DEFAULT;
mVcount = 0;
mVertices = 0;
mVertexStride = sizeof(btVector3);
mNormalEpsilon = 0.001f;
mMaxVertices = 4096; // maximum number of points to be considered for a convex hull.
mMaxFaces = 4096;
mFlags = QF_DEFAULT;
mVcount = 0;
mVertices = 0;
mVertexStride = sizeof(btVector3);
mNormalEpsilon = 0.001f;
mMaxVertices = 4096; // maximum number of points to be considered for a convex hull.
mMaxFaces = 4096;
};
HullDesc(HullFlag flag,
unsigned int vcount,
const btVector3 *vertices,
unsigned int stride = sizeof(btVector3))
unsigned int vcount,
const btVector3* vertices,
unsigned int stride = sizeof(btVector3))
{
mFlags = flag;
mVcount = vcount;
mVertices = vertices;
mVertexStride = stride;
mNormalEpsilon = btScalar(0.001);
mMaxVertices = 4096;
mFlags = flag;
mVcount = vcount;
mVertices = vertices;
mVertexStride = stride;
mNormalEpsilon = btScalar(0.001);
mMaxVertices = 4096;
}
bool HasHullFlag(HullFlag flag) const
{
if ( mFlags & flag ) return true;
if (mFlags & flag) return true;
return false;
}
void SetHullFlag(HullFlag flag)
{
mFlags|=flag;
mFlags |= flag;
}
void ClearHullFlag(HullFlag flag)
{
mFlags&=~flag;
mFlags &= ~flag;
}
unsigned int mFlags; // flags to use when generating the convex hull.
unsigned int mVcount; // number of vertices in the input point cloud
const btVector3 *mVertices; // the array of vertices.
unsigned int mVertexStride; // the stride of each vertex, in bytes.
btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull!
unsigned int mMaxFaces;
unsigned int mFlags; // flags to use when generating the convex hull.
unsigned int mVcount; // number of vertices in the input point cloud
const btVector3* mVertices; // the array of vertices.
unsigned int mVertexStride; // the stride of each vertex, in bytes.
btScalar mNormalEpsilon; // the epsilon for removing duplicates. This is a normalized value, if normalized bit is on.
unsigned int mMaxVertices; // maximum number of vertices to be considered for the hull!
unsigned int mMaxFaces;
};
enum HullError
{
QE_OK, // success!
QE_FAIL // failed.
QE_OK, // success!
QE_FAIL // failed.
};
class btPlane
{
public:
btVector3 normal;
btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0
btPlane(const btVector3 &n,btScalar d):normal(n),dist(d){}
btPlane():normal(),dist(0){}
public:
btVector3 normal;
btScalar dist; // distance below origin - the D from plane equasion Ax+By+Cz+D=0
btPlane(const btVector3& n, btScalar d) : normal(n), dist(d) {}
btPlane() : normal(), dist(0) {}
};
class ConvexH
class ConvexH
{
public:
public:
class HalfEdge
{
public:
public:
short ea; // the other half of the edge (index into edges list)
unsigned char v; // the vertex at the start of this edge (index into vertices list)
unsigned char p; // the facet on which this edge lies (index into facets list)
HalfEdge(){}
HalfEdge(short _ea,unsigned char _v, unsigned char _p):ea(_ea),v(_v),p(_p){}
HalfEdge() {}
HalfEdge(short _ea, unsigned char _v, unsigned char _p) : ea(_ea), v(_v), p(_p) {}
};
ConvexH()
{
@@ -143,25 +139,29 @@ class ConvexH
}
btAlignedObjectArray<btVector3> vertices;
btAlignedObjectArray<HalfEdge> edges;
btAlignedObjectArray<btPlane> facets;
ConvexH(int vertices_size,int edges_size,int facets_size);
btAlignedObjectArray<btPlane> facets;
ConvexH(int vertices_size, int edges_size, int facets_size);
};
class int4
{
public:
int x,y,z,w;
int x, y, z, w;
int4(){};
int4(int _x,int _y, int _z,int _w){x=_x;y=_y;z=_z;w=_w;}
const int& operator[](int i) const {return (&x)[i];}
int& operator[](int i) {return (&x)[i];}
int4(int _x, int _y, int _z, int _w)
{
x = _x;
y = _y;
z = _z;
w = _w;
}
const int& operator[](int i) const { return (&x)[i]; }
int& operator[](int i) { return (&x)[i]; }
};
class PHullResult
{
public:
PHullResult(void)
{
mVcount = 0;
@@ -173,69 +173,61 @@ public:
unsigned int mVcount;
unsigned int mIndexCount;
unsigned int mFaceCount;
btVector3* mVertices;
btVector3* mVertices;
TUIntArray m_Indices;
};
///The HullLibrary class can create a convex hull from a collection of vertices, using the ComputeHull method.
///The btShapeHull class uses this HullLibrary to create a approximate convex mesh given a general (non-polyhedral) convex shape.
class HullLibrary
{
btAlignedObjectArray<class btHullTriangle*> m_tris;
public:
btAlignedObjectArray<int> m_vertexIndexMapping;
HullError CreateConvexHull(const HullDesc& desc, // describes the input request
HullResult& result); // contains the resulst
HullError ReleaseResult(HullResult &result); // release memory allocated for this result, we are done with it.
HullError CreateConvexHull(const HullDesc& desc, // describes the input request
HullResult& result); // contains the resulst
HullError ReleaseResult(HullResult& result); // release memory allocated for this result, we are done with it.
private:
bool ComputeHull(unsigned int vcount, const btVector3* vertices, PHullResult& result, unsigned int vlimit);
bool ComputeHull(unsigned int vcount,const btVector3 *vertices,PHullResult &result,unsigned int vlimit);
class btHullTriangle* allocateTriangle(int a, int b, int c);
void deAllocateTriangle(btHullTriangle*);
void b2bfix(btHullTriangle* s, btHullTriangle* t);
class btHullTriangle* allocateTriangle(int a,int b,int c);
void deAllocateTriangle(btHullTriangle*);
void b2bfix(btHullTriangle* s,btHullTriangle*t);
void removeb2b(btHullTriangle* s, btHullTriangle* t);
void removeb2b(btHullTriangle* s,btHullTriangle*t);
void checkit(btHullTriangle *t);
void checkit(btHullTriangle* t);
btHullTriangle* extrudable(btScalar epsilon);
int calchull(btVector3 *verts,int verts_count, TUIntArray& tris_out, int &tris_count,int vlimit);
int calchull(btVector3* verts, int verts_count, TUIntArray& tris_out, int& tris_count, int vlimit);
int calchullgen(btVector3 *verts,int verts_count, int vlimit);
int calchullgen(btVector3* verts, int verts_count, int vlimit);
int4 FindSimplex(btVector3 *verts,int verts_count,btAlignedObjectArray<int> &allow);
int4 FindSimplex(btVector3* verts, int verts_count, btAlignedObjectArray<int>& allow);
class ConvexH* ConvexHCrop(ConvexH& convex,const btPlane& slice);
class ConvexH* ConvexHCrop(ConvexH& convex, const btPlane& slice);
void extrude(class btHullTriangle* t0,int v);
void extrude(class btHullTriangle* t0, int v);
ConvexH* test_cube();
//BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'.
//BringOutYourDead (John Ratcliff): When you create a convex hull you hand it a large input set of vertices forming a 'point cloud'.
//After the hull is generated it give you back a set of polygon faces which index the *original* point cloud.
//The thing is, often times, there are many 'dead vertices' in the point cloud that are on longer referenced by the hull.
//The routine 'BringOutYourDead' find only the referenced vertices, copies them to an new buffer, and re-indexes the hull so that it is a minimal representation.
void BringOutYourDead(const btVector3* verts,unsigned int vcount, btVector3* overts,unsigned int &ocount,unsigned int* indices,unsigned indexcount);
void BringOutYourDead(const btVector3* verts, unsigned int vcount, btVector3* overts, unsigned int& ocount, unsigned int* indices, unsigned indexcount);
bool CleanupVertices(unsigned int svcount,
const btVector3* svertices,
unsigned int stride,
unsigned int &vcount, // output number of vertices
btVector3* vertices, // location to store the results.
btScalar normalepsilon,
btVector3& scale);
const btVector3* svertices,
unsigned int stride,
unsigned int& vcount, // output number of vertices
btVector3* vertices, // location to store the results.
btScalar normalepsilon,
btVector3& scale);
};
#endif //BT_CD_HULL_H
#endif //BT_CD_HULL_H

1786
src/LinearMath/btConvexHullComputer.cpp
View File

File diff suppressed because it is too large Load Diff

100
src/LinearMath/btConvexHullComputer.h
View File

@@ -23,58 +23,56 @@ subject to the following restrictions:
/// Ole Kniemeyer, MAXON Computer GmbH
class btConvexHullComputer
{
private:
btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp);
public:
class Edge
{
private:
btScalar compute(const void* coords, bool doubleCoords, int stride, int count, btScalar shrink, btScalar shrinkClamp);
int next;
int reverse;
int targetVertex;
friend class btConvexHullComputer;
public:
class Edge
int getSourceVertex() const
{
private:
int next;
int reverse;
int targetVertex;
return (this + reverse)->targetVertex;
}
friend class btConvexHullComputer;
int getTargetVertex() const
{
return targetVertex;
}
public:
int getSourceVertex() const
{
return (this + reverse)->targetVertex;
}
const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
{
return this + next;
}
int getTargetVertex() const
{
return targetVertex;
}
const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
{
return (this + reverse)->getNextEdgeOfVertex();
}
const Edge* getNextEdgeOfVertex() const // clockwise list of all edges of a vertex
{
return this + next;
}
const Edge* getReverseEdge() const
{
return this + reverse;
}
};
const Edge* getNextEdgeOfFace() const // counter-clockwise list of all edges of a face
{
return (this + reverse)->getNextEdgeOfVertex();
}
// Vertices of the output hull
btAlignedObjectArray<btVector3> vertices;
const Edge* getReverseEdge() const
{
return this + reverse;
}
};
// Edges of the output hull
btAlignedObjectArray<Edge> edges;
// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
btAlignedObjectArray<int> faces;
// Vertices of the output hull
btAlignedObjectArray<btVector3> vertices;
// Edges of the output hull
btAlignedObjectArray<Edge> edges;
// Faces of the convex hull. Each entry is an index into the "edges" array pointing to an edge of the face. Faces are planar n-gons
btAlignedObjectArray<int> faces;
/*
/*
Compute convex hull of "count" vertices stored in "coords". "stride" is the difference in bytes
between the addresses of consecutive vertices. If "shrink" is positive, the convex hull is shrunken
by that amount (each face is moved by "shrink" length units towards the center along its normal).
@@ -86,18 +84,16 @@ class btConvexHullComputer
The output convex hull can be found in the member variables "vertices", "edges", "faces".
*/
btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
{
return compute(coords, false, stride, count, shrink, shrinkClamp);
}
btScalar compute(const float* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
{
return compute(coords, false, stride, count, shrink, shrinkClamp);
}
// same as above, but double precision
btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
{
return compute(coords, true, stride, count, shrink, shrinkClamp);
}
// same as above, but double precision
btScalar compute(const double* coords, int stride, int count, btScalar shrink, btScalar shrinkClamp)
{
return compute(coords, true, stride, count, shrink, shrinkClamp);
}
};
#endif //BT_CONVEX_HULL_COMPUTER_H
#endif //BT_CONVEX_HULL_COMPUTER_H

38
src/LinearMath/btCpuFeatureUtility.h
View File

@@ -4,20 +4,20 @@
#include "LinearMath/btScalar.h"
#include <string.h>//memset
#ifdef USE_SIMD
#include <string.h> //memset
#ifdef USE_SIMD
#include <emmintrin.h>
#ifdef BT_ALLOW_SSE4
#include <intrin.h>
#endif //BT_ALLOW_SSE4
#endif //USE_SIMD
#endif //BT_ALLOW_SSE4
#endif //USE_SIMD
#if defined BT_USE_NEON
#define ARM_NEON_GCC_COMPATIBILITY 1
#define ARM_NEON_GCC_COMPATIBILITY 1
#include <arm_neon.h>
#include <sys/types.h>
#include <sys/sysctl.h> //for sysctlbyname
#endif //BT_USE_NEON
#include <sys/sysctl.h> //for sysctlbyname
#endif //BT_USE_NEON
///Rudimentary btCpuFeatureUtility for CPU features: only report the features that Bullet actually uses (SSE4/FMA3, NEON_HPFP)
///We assume SSE2 in case BT_USE_SSE2 is defined in LinearMath/btScalar.h
@@ -26,14 +26,13 @@ class btCpuFeatureUtility
public:
enum btCpuFeature
{
CPU_FEATURE_FMA3=1,
CPU_FEATURE_SSE4_1=2,
CPU_FEATURE_NEON_HPFP=4
CPU_FEATURE_FMA3 = 1,
CPU_FEATURE_SSE4_1 = 2,
CPU_FEATURE_NEON_HPFP = 4
};
static int getCpuFeatures()
{
static int capabilities = 0;
static bool testedCapabilities = false;
if (0 != testedCapabilities)
@@ -49,15 +48,15 @@ public:
if (0 == err && hasFeature)
capabilities |= CPU_FEATURE_NEON_HPFP;
}
#endif //BT_USE_NEON
#endif //BT_USE_NEON
#ifdef BT_ALLOW_SSE4
#ifdef BT_ALLOW_SSE4
{
int cpuInfo[4];
int cpuInfo[4];
memset(cpuInfo, 0, sizeof(cpuInfo));
unsigned long long sseExt = 0;
unsigned long long sseExt = 0;
__cpuid(cpuInfo, 1);
bool osUsesXSAVE_XRSTORE = cpuInfo[2] & (1 << 27) || false;
bool cpuAVXSuport = cpuInfo[2] & (1 << 28) || false;
@@ -79,14 +78,11 @@ public:
capabilities |= btCpuFeatureUtility::CPU_FEATURE_SSE4_1;
}
}
#endif//BT_ALLOW_SSE4
#endif //BT_ALLOW_SSE4
testedCapabilities = true;
return capabilities;
}
};
#endif //BT_CPU_UTILITY_H
#endif //BT_CPU_UTILITY_H

28
src/LinearMath/btDefaultMotionState.h
View File

@@ -4,39 +4,37 @@
#include "btMotionState.h"
///The btDefaultMotionState provides a common implementation to synchronize world transforms with offsets.
ATTRIBUTE_ALIGNED16(struct) btDefaultMotionState : public btMotionState
ATTRIBUTE_ALIGNED16(struct)
btDefaultMotionState : public btMotionState
{
btTransform m_graphicsWorldTrans;
btTransform m_centerOfMassOffset;
btTransform m_centerOfMassOffset;
btTransform m_startWorldTrans;
void* m_userPointer;
void* m_userPointer;
BT_DECLARE_ALIGNED_ALLOCATOR();
btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(),const btTransform& centerOfMassOffset = btTransform::getIdentity())
btDefaultMotionState(const btTransform& startTrans = btTransform::getIdentity(), const btTransform& centerOfMassOffset = btTransform::getIdentity())
: m_graphicsWorldTrans(startTrans),
m_centerOfMassOffset(centerOfMassOffset),
m_startWorldTrans(startTrans),
m_userPointer(0)
m_centerOfMassOffset(centerOfMassOffset),
m_startWorldTrans(startTrans),
m_userPointer(0)
{
}
///synchronizes world transform from user to physics
virtual void getWorldTransform(btTransform& centerOfMassWorldTrans ) const
virtual void getWorldTransform(btTransform & centerOfMassWorldTrans) const
{
centerOfMassWorldTrans = m_graphicsWorldTrans * m_centerOfMassOffset.inverse() ;
centerOfMassWorldTrans = m_graphicsWorldTrans * m_centerOfMassOffset.inverse();
}
///synchronizes world transform from physics to user
///Bullet only calls the update of worldtransform for active objects
virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans)
virtual void setWorldTransform(const btTransform& centerOfMassWorldTrans)
{
m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset;
m_graphicsWorldTrans = centerOfMassWorldTrans * m_centerOfMassOffset;
}
};
#endif //BT_DEFAULT_MOTION_STATE_H
#endif //BT_DEFAULT_MOTION_STATE_H

113
src/LinearMath/btGeometryUtil.cpp
View File

@@ -12,49 +12,43 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#include "btGeometryUtil.h"
/*
Make sure this dummy function never changes so that it
can be used by probes that are checking whether the
library is actually installed.
*/
extern "C"
{
void btBulletMathProbe ();
{
void btBulletMathProbe();
void btBulletMathProbe () {}
void btBulletMathProbe() {}
}
bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin)
bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin)
{
int numbrushes = planeEquations.size();
for (int i=0;i<numbrushes;i++)
for (int i = 0; i < numbrushes; i++)
{
const btVector3& N1 = planeEquations[i];
btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin;
if (dist>btScalar(0.))
btScalar dist = btScalar(N1.dot(point)) + btScalar(N1[3]) - margin;
if (dist > btScalar(0.))
{
return false;
}
}
return true;
}
bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin)
bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin)
{
int numvertices = vertices.size();
for (int i=0;i<numvertices;i++)
for (int i = 0; i < numvertices; i++)
{
const btVector3& N1 = vertices[i];
btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
if (dist>btScalar(0.))
btScalar dist = btScalar(planeNormal.dot(N1)) + btScalar(planeNormal[3]) - margin;
if (dist > btScalar(0.))
{
return false;
}
@@ -62,102 +56,98 @@ bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const
return true;
}
bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations);
bool notExist(const btVector3& planeEquation, const btAlignedObjectArray<btVector3>& planeEquations);
bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
bool notExist(const btVector3& planeEquation, const btAlignedObjectArray<btVector3>& planeEquations)
{
int numbrushes = planeEquations.size();
for (int i=0;i<numbrushes;i++)
for (int i = 0; i < numbrushes; i++)
{
const btVector3& N1 = planeEquations[i];
if (planeEquation.dot(N1) > btScalar(0.999))
{
return false;
}
}
}
return true;
}
void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut)
{
const int numvertices = vertices.size();
const int numvertices = vertices.size();
// brute force:
for (int i=0;i<numvertices;i++)
for (int i = 0; i < numvertices; i++)
{
const btVector3& N1 = vertices[i];
for (int j=i+1;j<numvertices;j++)
for (int j = i + 1; j < numvertices; j++)
{
const btVector3& N2 = vertices[j];
for (int k=j+1;k<numvertices;k++)
{
for (int k = j + 1; k < numvertices; k++)
{
const btVector3& N3 = vertices[k];
btVector3 planeEquation,edge0,edge1;
edge0 = N2-N1;
edge1 = N3-N1;
btVector3 planeEquation, edge0, edge1;
edge0 = N2 - N1;
edge1 = N3 - N1;
btScalar normalSign = btScalar(1.);
for (int ww=0;ww<2;ww++)
for (int ww = 0; ww < 2; ww++)
{
planeEquation = normalSign * edge0.cross(edge1);
if (planeEquation.length2() > btScalar(0.0001))
{
planeEquation.normalize();
if (notExist(planeEquation,planeEquationsOut))
if (notExist(planeEquation, planeEquationsOut))
{
planeEquation[3] = -planeEquation.dot(N1);
//check if inside, and replace supportingVertexOut if needed
if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
{
planeEquationsOut.push_back(planeEquation);
}
//check if inside, and replace supportingVertexOut if needed
if (areVerticesBehindPlane(planeEquation, vertices, btScalar(0.01)))
{
planeEquationsOut.push_back(planeEquation);
}
}
}
normalSign = btScalar(-1.);
}
}
}
}
}
void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations, btAlignedObjectArray<btVector3>& verticesOut)
{
const int numbrushes = planeEquations.size();
// brute force:
for (int i=0;i<numbrushes;i++)
for (int i = 0; i < numbrushes; i++)
{
const btVector3& N1 = planeEquations[i];
for (int j=i+1;j<numbrushes;j++)
for (int j = i + 1; j < numbrushes; j++)
{
const btVector3& N2 = planeEquations[j];
for (int k=j+1;k<numbrushes;k++)
{
for (int k = j + 1; k < numbrushes; k++)
{
const btVector3& N3 = planeEquations[k];
btVector3 n2n3; n2n3 = N2.cross(N3);
btVector3 n3n1; n3n1 = N3.cross(N1);
btVector3 n1n2; n1n2 = N1.cross(N2);
if ( ( n2n3.length2() > btScalar(0.0001) ) &&
( n3n1.length2() > btScalar(0.0001) ) &&
( n1n2.length2() > btScalar(0.0001) ) )
btVector3 n2n3;
n2n3 = N2.cross(N3);
btVector3 n3n1;
n3n1 = N3.cross(N1);
btVector3 n1n2;
n1n2 = N1.cross(N2);
if ((n2n3.length2() > btScalar(0.0001)) &&
(n3n1.length2() > btScalar(0.0001)) &&
(n1n2.length2() > btScalar(0.0001)))
{
//point P out of 3 plane equations:
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
//P = -------------------------------------------------------------------------
// N1 . ( N2 * N3 )
// d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 )
//P = -------------------------------------------------------------------------
// N1 . ( N2 * N3 )
btScalar quotient = (N1.dot(n2n3));
if (btFabs(quotient) > btScalar(0.000001))
@@ -172,7 +162,7 @@ void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<bt
potentialVertex *= quotient;
//check if inside, and replace supportingVertexOut if needed
if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
if (isPointInsidePlanes(planeEquations, potentialVertex, btScalar(0.01)))
{
verticesOut.push_back(potentialVertex);
}
@@ -182,4 +172,3 @@ void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<bt
}
}
}

22
src/LinearMath/btGeometryUtil.h
View File

@@ -12,7 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_GEOMETRY_UTIL_H
#define BT_GEOMETRY_UTIL_H
@@ -22,21 +21,16 @@ subject to the following restrictions:
///The btGeometryUtil helper class provides a few methods to convert between plane equations and vertices.
class btGeometryUtil
{
public:
static void getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut );
public:
static void getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut);
static void getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut );
static bool isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, btScalar margin);
static bool isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin);
static void getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations, btAlignedObjectArray<btVector3>& verticesOut);
static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin);
static bool isInside(const btAlignedObjectArray<btVector3>& vertices, const btVector3& planeNormal, btScalar margin);
static bool isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin);
static bool areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin);
};
#endif //BT_GEOMETRY_UTIL_H
#endif //BT_GEOMETRY_UTIL_H

91
src/LinearMath/btGrahamScan2dConvexHull.h
View File

@@ -13,41 +13,40 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef GRAHAM_SCAN_2D_CONVEX_HULL_H
#define GRAHAM_SCAN_2D_CONVEX_HULL_H
#include "btVector3.h"
#include "btAlignedObjectArray.h"
struct GrahamVector3 : public btVector3
{
GrahamVector3(const btVector3& org, int orgIndex)
:btVector3(org),
m_orgIndex(orgIndex)
: btVector3(org),
m_orgIndex(orgIndex)
{
}
btScalar m_angle;
btScalar m_angle;
int m_orgIndex;
};
struct btAngleCompareFunc {
struct btAngleCompareFunc
{
btVector3 m_anchor;
btAngleCompareFunc(const btVector3& anchor)
: m_anchor(anchor)
: m_anchor(anchor)
{
}
bool operator()(const GrahamVector3& a, const GrahamVector3& b) const {
bool operator()(const GrahamVector3& a, const GrahamVector3& b) const
{
if (a.m_angle != b.m_angle)
return a.m_angle < b.m_angle;
else
{
btScalar al = (a-m_anchor).length2();
btScalar bl = (b-m_anchor).length2();
btScalar al = (a - m_anchor).length2();
btScalar bl = (b - m_anchor).length2();
if (al != bl)
return al < bl;
return al < bl;
else
{
return a.m_orgIndex < b.m_orgIndex;
@@ -58,73 +57,73 @@ struct btAngleCompareFunc {
inline void GrahamScanConvexHull2D(btAlignedObjectArray<GrahamVector3>& originalPoints, btAlignedObjectArray<GrahamVector3>& hull, const btVector3& normalAxis)
{
btVector3 axis0,axis1;
btPlaneSpace1(normalAxis,axis0,axis1);
btVector3 axis0, axis1;
btPlaneSpace1(normalAxis, axis0, axis1);
if (originalPoints.size()<=1)
if (originalPoints.size() <= 1)
{
for (int i=0;i<originalPoints.size();i++)
for (int i = 0; i < originalPoints.size(); i++)
hull.push_back(originalPoints[0]);
return;
}
//step1 : find anchor point with smallest projection on axis0 and move it to first location
for (int i=0;i<originalPoints.size();i++)
for (int i = 0; i < originalPoints.size(); i++)
{
// const btVector3& left = originalPoints[i];
// const btVector3& right = originalPoints[0];
// const btVector3& left = originalPoints[i];
// const btVector3& right = originalPoints[0];
btScalar projL = originalPoints[i].dot(axis0);
btScalar projR = originalPoints[0].dot(axis0);
if (projL < projR)
{
originalPoints.swap(0,i);
originalPoints.swap(0, i);
}
}
//also precompute angles
originalPoints[0].m_angle = -1e30f;
for (int i=1;i<originalPoints.size();i++)
for (int i = 1; i < originalPoints.size(); i++)
{
btVector3 ar = originalPoints[i]-originalPoints[0];
btScalar ar1 = axis1.dot(ar);
btScalar ar0 = axis0.dot(ar);
if( ar1*ar1+ar0*ar0 < FLT_EPSILON )
{
originalPoints[i].m_angle = 0.0f;
}
else
{
originalPoints[i].m_angle = btAtan2Fast(ar1, ar0);
}
btVector3 ar = originalPoints[i] - originalPoints[0];
btScalar ar1 = axis1.dot(ar);
btScalar ar0 = axis0.dot(ar);
if (ar1 * ar1 + ar0 * ar0 < FLT_EPSILON)
{
originalPoints[i].m_angle = 0.0f;
}
else
{
originalPoints[i].m_angle = btAtan2Fast(ar1, ar0);
}
}
//step 2: sort all points, based on 'angle' with this anchor
btAngleCompareFunc comp(originalPoints[0]);
originalPoints.quickSortInternal(comp,1,originalPoints.size()-1);
originalPoints.quickSortInternal(comp, 1, originalPoints.size() - 1);
int i;
for (i = 0; i<2; i++)
for (i = 0; i < 2; i++)
hull.push_back(originalPoints[i]);
//step 3: keep all 'convex' points and discard concave points (using back tracking)
for (; i != originalPoints.size(); i++)
for (; i != originalPoints.size(); i++)
{
bool isConvex = false;
while (!isConvex&& hull.size()>1) {
btVector3& a = hull[hull.size()-2];
btVector3& b = hull[hull.size()-1];
isConvex = btCross(a-b,a-originalPoints[i]).dot(normalAxis)> 0;
while (!isConvex && hull.size() > 1)
{
btVector3& a = hull[hull.size() - 2];
btVector3& b = hull[hull.size() - 1];
isConvex = btCross(a - b, a - originalPoints[i]).dot(normalAxis) > 0;
if (!isConvex)
hull.pop_back();
else
else
hull.push_back(originalPoints[i]);
}
if( hull.size() == 1 )
{
hull.push_back( originalPoints[i] );
}
if (hull.size() == 1)
{
hull.push_back(originalPoints[i]);
}
}
}
#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H
#endif //GRAHAM_SCAN_2D_CONVEX_HULL_H

244
src/LinearMath/btHashMap.h
View File

@@ -13,7 +13,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_HASH_MAP_H
#define BT_HASH_MAP_H
@@ -24,32 +23,32 @@ subject to the following restrictions:
struct btHashString
{
std::string m_string1;
unsigned int m_hash;
unsigned int m_hash;
SIMD_FORCE_INLINE unsigned int getHash()const
SIMD_FORCE_INLINE unsigned int getHash() const
{
return m_hash;
}
btHashString()
{
m_string1="";
m_hash=0;
m_string1 = "";
m_hash = 0;
}
btHashString(const char* name)
:m_string1(name)
: m_string1(name)
{
/* magic numbers from http://www.isthe.com/chongo/tech/comp/fnv/ */
static const unsigned int InitialFNV = 2166136261u;
static const unsigned int InitialFNV = 2166136261u;
static const unsigned int FNVMultiple = 16777619u;
/* Fowler / Noll / Vo (FNV) Hash */
unsigned int hash = InitialFNV;
for(int i = 0; m_string1.c_str()[i]; i++)
for (int i = 0; m_string1.c_str()[i]; i++)
{
hash = hash ^ (m_string1.c_str()[i]); /* xor the low 8 bits */
hash = hash * FNVMultiple; /* multiply by the magic number */
hash = hash ^ (m_string1.c_str()[i]); /* xor the low 8 bits */
hash = hash * FNVMultiple; /* multiply by the magic number */
}
m_hash = hash;
}
@@ -60,28 +59,27 @@ struct btHashString
}
};
const int BT_HASH_NULL=0xffffffff;
const int BT_HASH_NULL = 0xffffffff;
class btHashInt
{
int m_uid;
public:
int m_uid;
public:
btHashInt()
{
}
btHashInt(int uid) :m_uid(uid)
btHashInt(int uid) : m_uid(uid)
{
}
int getUid1() const
int getUid1() const
{
return m_uid;
}
void setUid1(int uid)
void setUid1(int uid)
{
m_uid = uid;
}
@@ -91,35 +89,35 @@ public:
return getUid1() == other.getUid1();
}
//to our success
SIMD_FORCE_INLINE unsigned int getHash()const
SIMD_FORCE_INLINE unsigned int getHash() const
{
unsigned int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
class btHashPtr
{
union
{
const void* m_pointer;
unsigned int m_hashValues[2];
union {
const void* m_pointer;
unsigned int m_hashValues[2];
};
public:
btHashPtr(const void* ptr)
:m_pointer(ptr)
: m_pointer(ptr)
{
}
const void* getPointer() const
const void* getPointer() const
{
return m_pointer;
}
@@ -130,64 +128,68 @@ public:
}
//to our success
SIMD_FORCE_INLINE unsigned int getHash()const
SIMD_FORCE_INLINE unsigned int getHash() const
{
const bool VOID_IS_8 = ((sizeof(void*)==8));
unsigned int key = VOID_IS_8? m_hashValues[0]+m_hashValues[1] : m_hashValues[0];
const bool VOID_IS_8 = ((sizeof(void*) == 8));
unsigned int key = VOID_IS_8 ? m_hashValues[0] + m_hashValues[1] : m_hashValues[0];
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
template <class Value>
class btHashKeyPtr
{
int m_uid;
int m_uid;
public:
btHashKeyPtr(int uid) : m_uid(uid)
{
}
btHashKeyPtr(int uid) :m_uid(uid)
{
}
int getUid1() const
{
return m_uid;
}
int getUid1() const
{
return m_uid;
}
bool equals(const btHashKeyPtr<Value>& other) const
{
return getUid1() == other.getUid1();
}
bool equals(const btHashKeyPtr<Value>& other) const
{
return getUid1() == other.getUid1();
}
//to our success
SIMD_FORCE_INLINE unsigned int getHash()const
{
unsigned int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
return key;
}
//to our success
SIMD_FORCE_INLINE unsigned int getHash() const
{
unsigned int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
template <class Value>
class btHashKey
{
int m_uid;
public:
int m_uid;
btHashKey(int uid) :m_uid(uid)
public:
btHashKey(int uid) : m_uid(uid)
{
}
int getUid1() const
int getUid1() const
{
return m_uid;
}
@@ -197,30 +199,33 @@ public:
return getUid1() == other.getUid1();
}
//to our success
SIMD_FORCE_INLINE unsigned int getHash()const
SIMD_FORCE_INLINE unsigned int getHash() const
{
unsigned int key = m_uid;
// Thomas Wang's hash
key += ~(key << 15); key ^= (key >> 10); key += (key << 3); key ^= (key >> 6); key += ~(key << 11); key ^= (key >> 16);
key += ~(key << 15);
key ^= (key >> 10);
key += (key << 3);
key ^= (key >> 6);
key += ~(key << 11);
key ^= (key >> 16);
return key;
}
};
///The btHashMap template class implements a generic and lightweight hashmap.
///A basic sample of how to use btHashMap is located in Demos\BasicDemo\main.cpp
template <class Key, class Value>
class btHashMap
{
protected:
btAlignedObjectArray<int> m_hashTable;
btAlignedObjectArray<int> m_next;
btAlignedObjectArray<Value> m_valueArray;
btAlignedObjectArray<Key> m_keyArray;
btAlignedObjectArray<int> m_hashTable;
btAlignedObjectArray<int> m_next;
void growTables(const Key& /*key*/)
btAlignedObjectArray<Value> m_valueArray;
btAlignedObjectArray<Key> m_keyArray;
void growTables(const Key& /*key*/)
{
int newCapacity = m_valueArray.capacity();
@@ -234,7 +239,7 @@ protected:
int i;
for (i= 0; i < newCapacity; ++i)
for (i = 0; i < newCapacity; ++i)
{
m_hashTable[i] = BT_HASH_NULL;
}
@@ -243,30 +248,28 @@ protected:
m_next[i] = BT_HASH_NULL;
}
for(i=0;i<curHashtableSize;i++)
for (i = 0; i < curHashtableSize; i++)
{
//const Value& value = m_valueArray[i];
//const Key& key = m_keyArray[i];
int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity()-1); // New hash value with new mask
int hashValue = m_keyArray[i].getHash() & (m_valueArray.capacity() - 1); // New hash value with new mask
m_next[i] = m_hashTable[hashValue];
m_hashTable[hashValue] = i;
}
}
}
public:
void insert(const Key& key, const Value& value) {
int hash = key.getHash() & (m_valueArray.capacity()-1);
public:
void insert(const Key& key, const Value& value)
{
int hash = key.getHash() & (m_valueArray.capacity() - 1);
//replace value if the key is already there
int index = findIndex(key);
if (index != BT_HASH_NULL)
{
m_valueArray[index]=value;
m_valueArray[index] = value;
return;
}
@@ -280,19 +283,19 @@ protected:
{
growTables(key);
//hash with new capacity
hash = key.getHash() & (m_valueArray.capacity()-1);
hash = key.getHash() & (m_valueArray.capacity() - 1);
}
m_next[count] = m_hashTable[hash];
m_hashTable[hash] = count;
}
void remove(const Key& key) {
int hash = key.getHash() & (m_valueArray.capacity()-1);
void remove(const Key& key)
{
int hash = key.getHash() & (m_valueArray.capacity() - 1);
int pairIndex = findIndex(key);
if (pairIndex ==BT_HASH_NULL)
if (pairIndex == BT_HASH_NULL)
{
return;
}
@@ -333,7 +336,7 @@ protected:
}
// Remove the last pair from the hash table.
int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity()-1);
int lastHash = m_keyArray[lastPairIndex].getHash() & (m_valueArray.capacity() - 1);
index = m_hashTable[lastHash];
btAssert(index != BT_HASH_NULL);
@@ -365,10 +368,8 @@ protected:
m_valueArray.pop_back();
m_keyArray.pop_back();
}
int size() const
{
return m_valueArray.size();
@@ -377,8 +378,8 @@ protected:
const Value* getAtIndex(int index) const
{
btAssert(index < m_valueArray.size());
btAssert(index>=0);
if (index>=0 && index < m_valueArray.size())
btAssert(index >= 0);
if (index >= 0 && index < m_valueArray.size())
{
return &m_valueArray[index];
}
@@ -388,38 +389,39 @@ protected:
Value* getAtIndex(int index)
{
btAssert(index < m_valueArray.size());
btAssert(index>=0);
if (index>=0 && index < m_valueArray.size())
btAssert(index >= 0);
if (index >= 0 && index < m_valueArray.size())
{
return &m_valueArray[index];
}
return 0;
}
Key getKeyAtIndex(int index)
{
btAssert(index < m_keyArray.size());
btAssert(index>=0);
Key getKeyAtIndex(int index)
{
btAssert(index < m_keyArray.size());
btAssert(index >= 0);
return m_keyArray[index];
}
const Key getKeyAtIndex(int index) const
{
btAssert(index < m_keyArray.size());
btAssert(index>=0);
}
const Key getKeyAtIndex(int index) const
{
btAssert(index < m_keyArray.size());
btAssert(index >= 0);
return m_keyArray[index];
}
}
Value* operator[](const Key& key) {
Value* operator[](const Key& key)
{
return find(key);
}
const Value* operator[](const Key& key) const {
const Value* operator[](const Key& key) const
{
return find(key);
}
const Value* find(const Key& key) const
const Value* find(const Key& key) const
{
int index = findIndex(key);
if (index == BT_HASH_NULL)
@@ -429,7 +431,7 @@ protected:
return &m_valueArray[index];
}
Value* find(const Key& key)
Value* find(const Key& key)
{
int index = findIndex(key);
if (index == BT_HASH_NULL)
@@ -439,10 +441,9 @@ protected:
return &m_valueArray[index];
}
int findIndex(const Key& key) const
int findIndex(const Key& key) const
{
unsigned int hash = key.getHash() & (m_valueArray.capacity()-1);
unsigned int hash = key.getHash() & (m_valueArray.capacity() - 1);
if (hash >= (unsigned int)m_hashTable.size())
{
@@ -457,14 +458,13 @@ protected:
return index;
}
void clear()
void clear()
{
m_hashTable.clear();
m_next.clear();
m_valueArray.clear();
m_keyArray.clear();
}
};
#endif //BT_HASH_MAP_H
#endif //BT_HASH_MAP_H

352
src/LinearMath/btIDebugDraw.h
View File

@@ -13,86 +13,84 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_IDEBUG_DRAW__H
#define BT_IDEBUG_DRAW__H
#include "btVector3.h"
#include "btTransform.h"
///The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations.
///Typical use case: create a debug drawer object, and assign it to a btCollisionWorld or btDynamicsWorld using setDebugDrawer and call debugDrawWorld.
///A class that implements the btIDebugDraw interface has to implement the drawLine method at a minimum.
///For color arguments the X,Y,Z components refer to Red, Green and Blue each in the range [0..1]
class btIDebugDraw
class btIDebugDraw
{
public:
ATTRIBUTE_ALIGNED16(struct) DefaultColors
public:
ATTRIBUTE_ALIGNED16(struct)
DefaultColors
{
btVector3 m_activeObject;
btVector3 m_deactivatedObject;
btVector3 m_wantsDeactivationObject;
btVector3 m_disabledDeactivationObject;
btVector3 m_disabledSimulationObject;
btVector3 m_aabb;
btVector3 m_activeObject;
btVector3 m_deactivatedObject;
btVector3 m_wantsDeactivationObject;
btVector3 m_disabledDeactivationObject;
btVector3 m_disabledSimulationObject;
btVector3 m_aabb;
btVector3 m_contactPoint;
DefaultColors()
: m_activeObject(1,1,1),
m_deactivatedObject(0,1,0),
m_wantsDeactivationObject(0,1,1),
m_disabledDeactivationObject(1,0,0),
m_disabledSimulationObject(1,1,0),
m_aabb(1,0,0),
m_contactPoint(1,1,0)
: m_activeObject(1, 1, 1),
m_deactivatedObject(0, 1, 0),
m_wantsDeactivationObject(0, 1, 1),
m_disabledDeactivationObject(1, 0, 0),
m_disabledSimulationObject(1, 1, 0),
m_aabb(1, 0, 0),
m_contactPoint(1, 1, 0)
{
}
};
enum DebugDrawModes
enum DebugDrawModes
{
DBG_NoDebug=0,
DBG_NoDebug = 0,
DBG_DrawWireframe = 1,
DBG_DrawAabb=2,
DBG_DrawFeaturesText=4,
DBG_DrawContactPoints=8,
DBG_NoDeactivation=16,
DBG_DrawAabb = 2,
DBG_DrawFeaturesText = 4,
DBG_DrawContactPoints = 8,
DBG_NoDeactivation = 16,
DBG_NoHelpText = 32,
DBG_DrawText=64,
DBG_DrawText = 64,
DBG_ProfileTimings = 128,
DBG_EnableSatComparison = 256,
DBG_DisableBulletLCP = 512,
DBG_EnableCCD = 1024,
DBG_DrawConstraints = (1 << 11),
DBG_DrawConstraintLimits = (1 << 12),
DBG_FastWireframe = (1<<13),
DBG_DrawNormals = (1<<14),
DBG_DrawFrames = (1<<15),
DBG_FastWireframe = (1 << 13),
DBG_DrawNormals = (1 << 14),
DBG_DrawFrames = (1 << 15),
DBG_MAX_DEBUG_DRAW_MODE
};
virtual ~btIDebugDraw() {};
virtual ~btIDebugDraw(){};
virtual DefaultColors getDefaultColors() const { DefaultColors colors; return colors; }
virtual DefaultColors getDefaultColors() const
{
DefaultColors colors;
return colors;
}
///the default implementation for setDefaultColors has no effect. A derived class can implement it and store the colors.
virtual void setDefaultColors(const DefaultColors& /*colors*/) {}
virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)=0;
virtual void drawLine(const btVector3& from,const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& color) = 0;
virtual void drawLine(const btVector3& from, const btVector3& to, const btVector3& fromColor, const btVector3& toColor)
{
(void) toColor;
drawLine (from, to, fromColor);
(void)toColor;
drawLine(from, to, fromColor);
}
virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
virtual void drawSphere(btScalar radius, const btTransform& transform, const btVector3& color)
{
btVector3 center = transform.getOrigin();
btVector3 up = transform.getBasis().getColumn(1);
btVector3 axis = transform.getBasis().getColumn(0);
@@ -101,103 +99,102 @@ class btIDebugDraw
btScalar minPs = -SIMD_HALF_PI;
btScalar maxPs = SIMD_HALF_PI;
btScalar stepDegrees = 30.f;
drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees ,false);
drawSpherePatch(center, up, -axis, radius,minTh, maxTh, minPs, maxPs, color, stepDegrees,false );
drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, stepDegrees, false);
drawSpherePatch(center, up, -axis, radius, minTh, maxTh, minPs, maxPs, color, stepDegrees, false);
}
virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
virtual void drawSphere(const btVector3& p, btScalar radius, const btVector3& color)
{
btTransform tr;
tr.setIdentity();
tr.setOrigin(p);
drawSphere(radius,tr,color);
}
virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& /*n0*/,const btVector3& /*n1*/,const btVector3& /*n2*/,const btVector3& color, btScalar alpha)
{
drawTriangle(v0,v1,v2,color,alpha);
}
virtual void drawTriangle(const btVector3& v0,const btVector3& v1,const btVector3& v2,const btVector3& color, btScalar /*alpha*/)
{
drawLine(v0,v1,color);
drawLine(v1,v2,color);
drawLine(v2,v0,color);
drawSphere(radius, tr, color);
}
virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)=0;
virtual void reportErrorWarning(const char* warningString) = 0;
virtual void draw3dText(const btVector3& location,const char* textString) = 0;
virtual void setDebugMode(int debugMode) =0;
virtual int getDebugMode() const = 0;
virtual void drawAabb(const btVector3& from,const btVector3& to,const btVector3& color)
virtual void drawTriangle(const btVector3& v0, const btVector3& v1, const btVector3& v2, const btVector3& /*n0*/, const btVector3& /*n1*/, const btVector3& /*n2*/, const btVector3& color, btScalar alpha)
{
drawTriangle(v0, v1, v2, color, alpha);
}
virtual void drawTriangle(const btVector3& v0, const btVector3& v1, const btVector3& v2, const btVector3& color, btScalar /*alpha*/)
{
drawLine(v0, v1, color);
drawLine(v1, v2, color);
drawLine(v2, v0, color);
}
btVector3 halfExtents = (to-from)* 0.5f;
btVector3 center = (to+from) *0.5f;
int i,j;
virtual void drawContactPoint(const btVector3& PointOnB, const btVector3& normalOnB, btScalar distance, int lifeTime, const btVector3& color) = 0;
btVector3 edgecoord(1.f,1.f,1.f),pa,pb;
for (i=0;i<4;i++)
virtual void reportErrorWarning(const char* warningString) = 0;
virtual void draw3dText(const btVector3& location, const char* textString) = 0;
virtual void setDebugMode(int debugMode) = 0;
virtual int getDebugMode() const = 0;
virtual void drawAabb(const btVector3& from, const btVector3& to, const btVector3& color)
{
btVector3 halfExtents = (to - from) * 0.5f;
btVector3 center = (to + from) * 0.5f;
int i, j;
btVector3 edgecoord(1.f, 1.f, 1.f), pa, pb;
for (i = 0; i < 4; i++)
{
for (j=0;j<3;j++)
for (j = 0; j < 3; j++)
{
pa = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
edgecoord[2]*halfExtents[2]);
pa+=center;
pa = btVector3(edgecoord[0] * halfExtents[0], edgecoord[1] * halfExtents[1],
edgecoord[2] * halfExtents[2]);
pa += center;
int othercoord = j%3;
edgecoord[othercoord]*=-1.f;
pb = btVector3(edgecoord[0]*halfExtents[0], edgecoord[1]*halfExtents[1],
edgecoord[2]*halfExtents[2]);
pb+=center;
int othercoord = j % 3;
edgecoord[othercoord] *= -1.f;
pb = btVector3(edgecoord[0] * halfExtents[0], edgecoord[1] * halfExtents[1],
edgecoord[2] * halfExtents[2]);
pb += center;
drawLine(pa,pb,color);
drawLine(pa, pb, color);
}
edgecoord = btVector3(-1.f,-1.f,-1.f);
if (i<3)
edgecoord[i]*=-1.f;
edgecoord = btVector3(-1.f, -1.f, -1.f);
if (i < 3)
edgecoord[i] *= -1.f;
}
}
virtual void drawTransform(const btTransform& transform, btScalar orthoLen)
{
btVector3 start = transform.getOrigin();
drawLine(start, start+transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(btScalar(1.), btScalar(0.3), btScalar(0.3)));
drawLine(start, start+transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(btScalar(0.3), btScalar(1.), btScalar(0.3)));
drawLine(start, start+transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(btScalar(0.3), btScalar(0.3), btScalar(1.)));
drawLine(start, start + transform.getBasis() * btVector3(orthoLen, 0, 0), btVector3(btScalar(1.), btScalar(0.3), btScalar(0.3)));
drawLine(start, start + transform.getBasis() * btVector3(0, orthoLen, 0), btVector3(btScalar(0.3), btScalar(1.), btScalar(0.3)));
drawLine(start, start + transform.getBasis() * btVector3(0, 0, orthoLen), btVector3(btScalar(0.3), btScalar(0.3), btScalar(1.)));
}
virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
{
const btVector3& vx = axis;
btVector3 vy = normal.cross(axis);
btScalar step = stepDegrees * SIMD_RADS_PER_DEG;
int nSteps = (int)btFabs((maxAngle - minAngle) / step);
if(!nSteps) nSteps = 1;
if (!nSteps) nSteps = 1;
btVector3 prev = center + radiusA * vx * btCos(minAngle) + radiusB * vy * btSin(minAngle);
if(drawSect)
if (drawSect)
{
drawLine(center, prev, color);
}
for(int i = 1; i <= nSteps; i++)
for (int i = 1; i <= nSteps; i++)
{
btScalar angle = minAngle + (maxAngle - minAngle) * btScalar(i) / btScalar(nSteps);
btVector3 next = center + radiusA * vx * btCos(angle) + radiusB * vy * btSin(angle);
drawLine(prev, next, color);
prev = next;
}
if(drawSect)
if (drawSect)
{
drawLine(center, prev, color);
}
}
virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f),bool drawCenter = true)
virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f), bool drawCenter = true)
{
btVector3 vA[74];
btVector3 vB[74];
@@ -211,33 +208,33 @@ class btIDebugDraw
btVector3 jv = kv.cross(iv);
bool drawN = false;
bool drawS = false;
if(minTh <= -SIMD_HALF_PI)
if (minTh <= -SIMD_HALF_PI)
{
minTh = -SIMD_HALF_PI + step;
drawN = true;
}
if(maxTh >= SIMD_HALF_PI)
if (maxTh >= SIMD_HALF_PI)
{
maxTh = SIMD_HALF_PI - step;
drawS = true;
}
if(minTh > maxTh)
if (minTh > maxTh)
{
minTh = -SIMD_HALF_PI + step;
maxTh = SIMD_HALF_PI - step;
maxTh = SIMD_HALF_PI - step;
drawN = drawS = true;
}
int n_hor = (int)((maxTh - minTh) / step) + 1;
if(n_hor < 2) n_hor = 2;
if (n_hor < 2) n_hor = 2;
btScalar step_h = (maxTh - minTh) / btScalar(n_hor - 1);
bool isClosed = false;
if(minPs > maxPs)
if (minPs > maxPs)
{
minPs = -SIMD_PI + step;
maxPs = SIMD_PI;
maxPs = SIMD_PI;
isClosed = true;
}
else if((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
else if ((maxPs - minPs) >= SIMD_PI * btScalar(2.f))
{
isClosed = true;
}
@@ -246,63 +243,64 @@ class btIDebugDraw
isClosed = false;
}
int n_vert = (int)((maxPs - minPs) / step) + 1;
if(n_vert < 2) n_vert = 2;
if (n_vert < 2) n_vert = 2;
btScalar step_v = (maxPs - minPs) / btScalar(n_vert - 1);
for(int i = 0; i < n_hor; i++)
for (int i = 0; i < n_hor; i++)
{
btScalar th = minTh + btScalar(i) * step_h;
btScalar sth = radius * btSin(th);
btScalar cth = radius * btCos(th);
for(int j = 0; j < n_vert; j++)
for (int j = 0; j < n_vert; j++)
{
btScalar psi = minPs + btScalar(j) * step_v;
btScalar sps = btSin(psi);
btScalar cps = btCos(psi);
pvB[j] = center + cth * cps * iv + cth * sps * jv + sth * kv;
if(i)
if (i)
{
drawLine(pvA[j], pvB[j], color);
}
else if(drawS)
else if (drawS)
{
drawLine(spole, pvB[j], color);
}
if(j)
if (j)
{
drawLine(pvB[j-1], pvB[j], color);
drawLine(pvB[j - 1], pvB[j], color);
}
else
{
arcStart = pvB[j];
}
if((i == (n_hor - 1)) && drawN)
if ((i == (n_hor - 1)) && drawN)
{
drawLine(npole, pvB[j], color);
}
if (drawCenter)
{
if(isClosed)
if (isClosed)
{
if(j == (n_vert-1))
if (j == (n_vert - 1))
{
drawLine(arcStart, pvB[j], color);
}
}
else
{
if(((!i) || (i == (n_hor-1))) && ((!j) || (j == (n_vert-1))))
if (((!i) || (i == (n_hor - 1))) && ((!j) || (j == (n_vert - 1))))
{
drawLine(center, pvB[j], color);
}
}
}
}
pT = pvA; pvA = pvB; pvB = pT;
pT = pvA;
pvA = pvB;
pvB = pT;
}
}
virtual void drawBox(const btVector3& bbMin, const btVector3& bbMax, const btVector3& color)
{
drawLine(btVector3(bbMin[0], bbMin[1], bbMin[2]), btVector3(bbMax[0], bbMin[1], bbMin[2]), color);
@@ -338,31 +336,27 @@ class btIDebugDraw
{
int stepDegrees = 30;
btVector3 capStart(0.f,0.f,0.f);
btVector3 capStart(0.f, 0.f, 0.f);
capStart[upAxis] = -halfHeight;
btVector3 capEnd(0.f,0.f,0.f);
btVector3 capEnd(0.f, 0.f, 0.f);
capEnd[upAxis] = halfHeight;
// Draw the ends
{
btTransform childTransform = transform;
childTransform.getOrigin() = transform * capStart;
{
btVector3 center = childTransform.getOrigin();
btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3);
btVector3 up = childTransform.getBasis().getColumn((upAxis + 1) % 3);
btVector3 axis = -childTransform.getBasis().getColumn(upAxis);
btScalar minTh = -SIMD_HALF_PI;
btScalar maxTh = SIMD_HALF_PI;
btScalar minPs = -SIMD_HALF_PI;
btScalar maxPs = SIMD_HALF_PI;
drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false);
drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees), false);
}
}
{
@@ -370,52 +364,51 @@ class btIDebugDraw
childTransform.getOrigin() = transform * capEnd;
{
btVector3 center = childTransform.getOrigin();
btVector3 up = childTransform.getBasis().getColumn((upAxis+1)%3);
btVector3 up = childTransform.getBasis().getColumn((upAxis + 1) % 3);
btVector3 axis = childTransform.getBasis().getColumn(upAxis);
btScalar minTh = -SIMD_HALF_PI;
btScalar maxTh = SIMD_HALF_PI;
btScalar minPs = -SIMD_HALF_PI;
btScalar maxPs = SIMD_HALF_PI;
drawSpherePatch(center, up, axis, radius,minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees) ,false);
drawSpherePatch(center, up, axis, radius, minTh, maxTh, minPs, maxPs, color, btScalar(stepDegrees), false);
}
}
// Draw some additional lines
btVector3 start = transform.getOrigin();
for (int i=0;i<360;i+=stepDegrees)
for (int i = 0; i < 360; i += stepDegrees)
{
capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
capEnd[(upAxis + 1) % 3] = capStart[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
capEnd[(upAxis + 2) % 3] = capStart[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
drawLine(start + transform.getBasis() * capStart, start + transform.getBasis() * capEnd, color);
}
}
virtual void drawCylinder(btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color)
{
btVector3 start = transform.getOrigin();
btVector3 offsetHeight(0,0,0);
btVector3 offsetHeight(0, 0, 0);
offsetHeight[upAxis] = halfHeight;
int stepDegrees=30;
btVector3 capStart(0.f,0.f,0.f);
int stepDegrees = 30;
btVector3 capStart(0.f, 0.f, 0.f);
capStart[upAxis] = -halfHeight;
btVector3 capEnd(0.f,0.f,0.f);
btVector3 capEnd(0.f, 0.f, 0.f);
capEnd[upAxis] = halfHeight;
for (int i=0;i<360;i+=stepDegrees)
for (int i = 0; i < 360; i += stepDegrees)
{
capEnd[(upAxis+1)%3] = capStart[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
capEnd[(upAxis+2)%3] = capStart[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
drawLine(start+transform.getBasis() * capStart,start+transform.getBasis() * capEnd, color);
capEnd[(upAxis + 1) % 3] = capStart[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
capEnd[(upAxis + 2) % 3] = capStart[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
drawLine(start + transform.getBasis() * capStart, start + transform.getBasis() * capEnd, color);
}
// Drawing top and bottom caps of the cylinder
btVector3 yaxis(0,0,0);
btVector3 yaxis(0, 0, 0);
yaxis[upAxis] = btScalar(1.0);
btVector3 xaxis(0,0,0);
xaxis[(upAxis+1)%3] = btScalar(1.0);
drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
drawArc(start+transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,btScalar(10.0));
btVector3 xaxis(0, 0, 0);
xaxis[(upAxis + 1) % 3] = btScalar(1.0);
drawArc(start - transform.getBasis() * (offsetHeight), transform.getBasis() * yaxis, transform.getBasis() * xaxis, radius, radius, 0, SIMD_2_PI, color, false, btScalar(10.0));
drawArc(start + transform.getBasis() * (offsetHeight), transform.getBasis() * yaxis, transform.getBasis() * xaxis, radius, radius, 0, SIMD_2_PI, color, false, btScalar(10.0));
}
virtual void drawCone(btScalar radius, btScalar height, int upAxis, const btTransform& transform, const btVector3& color)
@@ -423,50 +416,49 @@ class btIDebugDraw
int stepDegrees = 30;
btVector3 start = transform.getOrigin();
btVector3 offsetHeight(0,0,0);
btVector3 offsetHeight(0, 0, 0);
btScalar halfHeight = height * btScalar(0.5);
offsetHeight[upAxis] = halfHeight;
btVector3 offsetRadius(0,0,0);
offsetRadius[(upAxis+1)%3] = radius;
btVector3 offset2Radius(0,0,0);
offset2Radius[(upAxis+2)%3] = radius;
btVector3 offsetRadius(0, 0, 0);
offsetRadius[(upAxis + 1) % 3] = radius;
btVector3 offset2Radius(0, 0, 0);
offset2Radius[(upAxis + 2) % 3] = radius;
btVector3 capEnd(0.f,0.f,0.f);
btVector3 capEnd(0.f, 0.f, 0.f);
capEnd[upAxis] = -halfHeight;
for (int i=0;i<360;i+=stepDegrees)
for (int i = 0; i < 360; i += stepDegrees)
{
capEnd[(upAxis+1)%3] = btSin(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
capEnd[(upAxis+2)%3] = btCos(btScalar(i)*SIMD_RADS_PER_DEG)*radius;
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * capEnd, color);
capEnd[(upAxis + 1) % 3] = btSin(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
capEnd[(upAxis + 2) % 3] = btCos(btScalar(i) * SIMD_RADS_PER_DEG) * radius;
drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * capEnd, color);
}
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offsetRadius),color);
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offsetRadius),color);
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight+offset2Radius),color);
drawLine(start+transform.getBasis() * (offsetHeight),start+transform.getBasis() * (-offsetHeight-offset2Radius),color);
drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight + offsetRadius), color);
drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight - offsetRadius), color);
drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight + offset2Radius), color);
drawLine(start + transform.getBasis() * (offsetHeight), start + transform.getBasis() * (-offsetHeight - offset2Radius), color);
// Drawing the base of the cone
btVector3 yaxis(0,0,0);
btVector3 yaxis(0, 0, 0);
yaxis[upAxis] = btScalar(1.0);
btVector3 xaxis(0,0,0);
xaxis[(upAxis+1)%3] = btScalar(1.0);
drawArc(start-transform.getBasis()*(offsetHeight),transform.getBasis()*yaxis,transform.getBasis()*xaxis,radius,radius,0,SIMD_2_PI,color,false,10.0);
btVector3 xaxis(0, 0, 0);
xaxis[(upAxis + 1) % 3] = btScalar(1.0);
drawArc(start - transform.getBasis() * (offsetHeight), transform.getBasis() * yaxis, transform.getBasis() * xaxis, radius, radius, 0, SIMD_2_PI, color, false, 10.0);
}
virtual void drawPlane(const btVector3& planeNormal, btScalar planeConst, const btTransform& transform, const btVector3& color)
{
btVector3 planeOrigin = planeNormal * planeConst;
btVector3 vec0,vec1;
btPlaneSpace1(planeNormal,vec0,vec1);
btVector3 vec0, vec1;
btPlaneSpace1(planeNormal, vec0, vec1);
btScalar vecLen = 100.f;
btVector3 pt0 = planeOrigin + vec0*vecLen;
btVector3 pt1 = planeOrigin - vec0*vecLen;
btVector3 pt2 = planeOrigin + vec1*vecLen;
btVector3 pt3 = planeOrigin - vec1*vecLen;
drawLine(transform*pt0,transform*pt1,color);
drawLine(transform*pt2,transform*pt3,color);
btVector3 pt0 = planeOrigin + vec0 * vecLen;
btVector3 pt1 = planeOrigin - vec0 * vecLen;
btVector3 pt2 = planeOrigin + vec1 * vecLen;
btVector3 pt3 = planeOrigin - vec1 * vecLen;
drawLine(transform * pt0, transform * pt1, color);
drawLine(transform * pt2, transform * pt3, color);
}
virtual void clearLines()
@@ -478,6 +470,4 @@ class btIDebugDraw
}
};
#endif //BT_IDEBUG_DRAW__H
#endif //BT_IDEBUG_DRAW__H

86
src/LinearMath/btList.h
View File

@@ -12,62 +12,62 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_GEN_LIST_H
#define BT_GEN_LIST_H
class btGEN_Link {
class btGEN_Link
{
public:
btGEN_Link() : m_next(0), m_prev(0) {}
btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {}
btGEN_Link *getNext() const { return m_next; }
btGEN_Link *getPrev() const { return m_prev; }
btGEN_Link() : m_next(0), m_prev(0) {}
btGEN_Link(btGEN_Link *next, btGEN_Link *prev) : m_next(next), m_prev(prev) {}
bool isHead() const { return m_prev == 0; }
bool isTail() const { return m_next == 0; }
btGEN_Link *getNext() const { return m_next; }
btGEN_Link *getPrev() const { return m_prev; }
void insertBefore(btGEN_Link *link) {
m_next = link;
m_prev = link->m_prev;
m_next->m_prev = this;
m_prev->m_next = this;
}
bool isHead() const { return m_prev == 0; }
bool isTail() const { return m_next == 0; }
void insertAfter(btGEN_Link *link) {
m_next = link->m_next;
m_prev = link;
m_next->m_prev = this;
m_prev->m_next = this;
}
void insertBefore(btGEN_Link *link)
{
m_next = link;
m_prev = link->m_prev;
m_next->m_prev = this;
m_prev->m_next = this;
}
void remove() {
m_next->m_prev = m_prev;
m_prev->m_next = m_next;
}
void insertAfter(btGEN_Link *link)
{
m_next = link->m_next;
m_prev = link;
m_next->m_prev = this;
m_prev->m_next = this;
}
private:
btGEN_Link *m_next;
btGEN_Link *m_prev;
};
void remove()
{
m_next->m_prev = m_prev;
m_prev->m_next = m_next;
}
class btGEN_List {
public:
btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
btGEN_Link *getHead() const { return m_head.getNext(); }
btGEN_Link *getTail() const { return m_tail.getPrev(); }
void addHead(btGEN_Link *link) { link->insertAfter(&m_head); }
void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); }
private:
btGEN_Link m_head;
btGEN_Link m_tail;
btGEN_Link *m_next;
btGEN_Link *m_prev;
};
#endif //BT_GEN_LIST_H
class btGEN_List
{
public:
btGEN_List() : m_head(&m_tail, 0), m_tail(0, &m_head) {}
btGEN_Link *getHead() const { return m_head.getNext(); }
btGEN_Link *getTail() const { return m_tail.getPrev(); }
void addHead(btGEN_Link *link) { link->insertAfter(&m_head); }
void addTail(btGEN_Link *link) { link->insertBefore(&m_tail); }
private:
btGEN_Link m_head;
btGEN_Link m_tail;
};
#endif //BT_GEN_LIST_H

1444
src/LinearMath/btMatrix3x3.h
View File

File diff suppressed because it is too large Load Diff

389
src/LinearMath/btMatrixX.h
View File

@@ -24,24 +24,23 @@ subject to the following restrictions:
//#define BT_DEBUG_OSTREAM
#ifdef BT_DEBUG_OSTREAM
#include <iostream>
#include <iomanip> // std::setw
#endif //BT_DEBUG_OSTREAM
#include <iomanip> // std::setw
#endif //BT_DEBUG_OSTREAM
class btIntSortPredicate
{
public:
bool operator() ( const int& a, const int& b ) const
{
return a < b;
}
public:
bool operator()(const int& a, const int& b) const
{
return a < b;
}
};
template <typename T>
struct btVectorX
{
btAlignedObjectArray<T> m_storage;
btAlignedObjectArray<T> m_storage;
btVectorX()
{
}
@@ -49,7 +48,7 @@ struct btVectorX
{
m_storage.resize(numRows);
}
void resize(int rows)
{
m_storage.resize(rows);
@@ -66,13 +65,13 @@ struct btVectorX
{
return rows();
}
T nrm2() const
{
T norm = T(0);
int nn = rows();
{
if (nn == 1)
{
@@ -82,11 +81,11 @@ struct btVectorX
{
T scale = 0.0;
T ssq = 1.0;
/* The following loop is equivalent to this call to the LAPACK
auxiliary routine: CALL SLASSQ( N, X, INCX, SCALE, SSQ ) */
for (int ix=0;ix<nn;ix++)
for (int ix = 0; ix < nn; ix++)
{
if ((*this)[ix] != 0.0)
{
@@ -110,38 +109,36 @@ struct btVectorX
}
}
return norm;
}
void setZero()
void setZero()
{
if (m_storage.size())
{
// for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
btSetZero(&m_storage[0],m_storage.size());
btSetZero(&m_storage[0], m_storage.size());
}
}
const T& operator[] (int index) const
const T& operator[](int index) const
{
return m_storage[index];
}
T& operator[] (int index)
T& operator[](int index)
{
return m_storage[index];
}
T* getBufferPointerWritable()
{
return m_storage.size() ? &m_storage[0] : 0;
}
const T* getBufferPointer() const
{
return m_storage.size() ? &m_storage[0] : 0;
}
};
/*
template <typename T>
@@ -151,8 +148,7 @@ struct btVectorX
}
*/
template <typename T>
template <typename T>
struct btMatrixX
{
int m_rows;
@@ -161,10 +157,10 @@ struct btMatrixX
int m_resizeOperations;
int m_setElemOperations;
btAlignedObjectArray<T> m_storage;
mutable btAlignedObjectArray< btAlignedObjectArray<int> > m_rowNonZeroElements1;
btAlignedObjectArray<T> m_storage;
mutable btAlignedObjectArray<btAlignedObjectArray<int> > m_rowNonZeroElements1;
T* getBufferPointerWritable()
T* getBufferPointerWritable()
{
return m_storage.size() ? &m_storage[0] : 0;
}
@@ -174,21 +170,21 @@ struct btMatrixX
return m_storage.size() ? &m_storage[0] : 0;
}
btMatrixX()
:m_rows(0),
m_cols(0),
m_operations(0),
m_resizeOperations(0),
m_setElemOperations(0)
: m_rows(0),
m_cols(0),
m_operations(0),
m_resizeOperations(0),
m_setElemOperations(0)
{
}
btMatrixX(int rows,int cols)
:m_rows(rows),
m_cols(cols),
m_operations(0),
m_resizeOperations(0),
m_setElemOperations(0)
btMatrixX(int rows, int cols)
: m_rows(rows),
m_cols(cols),
m_operations(0),
m_resizeOperations(0),
m_setElemOperations(0)
{
resize(rows,cols);
resize(rows, cols);
}
void resize(int rows, int cols)
{
@@ -197,7 +193,7 @@ struct btMatrixX
m_cols = cols;
{
BT_PROFILE("m_storage.resize");
m_storage.resize(rows*cols);
m_storage.resize(rows * cols);
}
}
int cols() const
@@ -215,108 +211,99 @@ struct btMatrixX
}
*/
void addElem(int row,int col, T val)
void addElem(int row, int col, T val)
{
if (val)
{
if (m_storage[col+row*m_cols]==0.f)
if (m_storage[col + row * m_cols] == 0.f)
{
setElem(row,col,val);
} else
setElem(row, col, val);
}
else
{
m_storage[row*m_cols+col] += val;
m_storage[row * m_cols + col] += val;
}
}
}
void setElem(int row,int col, T val)
void setElem(int row, int col, T val)
{
m_setElemOperations++;
m_storage[row*m_cols+col] = val;
m_storage[row * m_cols + col] = val;
}
void mulElem(int row,int col, T val)
void mulElem(int row, int col, T val)
{
m_setElemOperations++;
//mul doesn't change sparsity info
m_storage[row*m_cols+col] *= val;
m_storage[row * m_cols + col] *= val;
}
void copyLowerToUpperTriangle()
{
int count=0;
for (int row=0;row<rows();row++)
int count = 0;
for (int row = 0; row < rows(); row++)
{
for (int col=0;col<row;col++)
for (int col = 0; col < row; col++)
{
setElem(col,row, (*this)(row,col));
setElem(col, row, (*this)(row, col));
count++;
}
}
//printf("copyLowerToUpperTriangle copied %d elements out of %dx%d=%d\n", count,rows(),cols(),cols()*rows());
}
const T& operator() (int row,int col) const
{
return m_storage[col+row*m_cols];
}
const T& operator()(int row, int col) const
{
return m_storage[col + row * m_cols];
}
void setZero()
{
{
BT_PROFILE("storage=0");
btSetZero(&m_storage[0],m_storage.size());
btSetZero(&m_storage[0], m_storage.size());
//memset(&m_storage[0],0,sizeof(T)*m_storage.size());
//for (int i=0;i<m_storage.size();i++)
// m_storage[i]=0;
// m_storage[i]=0;
}
}
void setIdentity()
{
btAssert(rows() == cols());
setZero();
for (int row=0;row<rows();row++)
for (int row = 0; row < rows(); row++)
{
setElem(row,row,1);
setElem(row, row, 1);
}
}
void printMatrix(const char* msg)
void printMatrix(const char* msg)
{
printf("%s ---------------------\n",msg);
for (int i=0;i<rows();i++)
printf("%s ---------------------\n", msg);
for (int i = 0; i < rows(); i++)
{
printf("\n");
for (int j=0;j<cols();j++)
for (int j = 0; j < cols(); j++)
{
printf("%2.1f\t",(*this)(i,j));
printf("%2.1f\t", (*this)(i, j));
}
}
printf("\n---------------------\n");
}
void rowComputeNonZeroElements() const
{
m_rowNonZeroElements1.resize(rows());
for (int i=0;i<rows();i++)
for (int i = 0; i < rows(); i++)
{
m_rowNonZeroElements1[i].resize(0);
for (int j=0;j<cols();j++)
for (int j = 0; j < cols(); j++)
{
if ((*this)(i,j)!=0.f)
if ((*this)(i, j) != 0.f)
{
m_rowNonZeroElements1[i].push_back(j);
}
@@ -326,54 +313,52 @@ struct btMatrixX
btMatrixX transpose() const
{
//transpose is optimized for sparse matrices
btMatrixX tr(m_cols,m_rows);
btMatrixX tr(m_cols, m_rows);
tr.setZero();
for (int i=0;i<m_cols;i++)
for (int j=0;j<m_rows;j++)
for (int i = 0; i < m_cols; i++)
for (int j = 0; j < m_rows; j++)
{
T v = (*this)(j,i);
T v = (*this)(j, i);
if (v)
{
tr.setElem(i,j,v);
tr.setElem(i, j, v);
}
}
return tr;
}
btMatrixX operator*(const btMatrixX& other)
{
//btMatrixX*btMatrixX implementation, brute force
btAssert(cols() == other.rows());
btMatrixX res(rows(),other.cols());
btMatrixX res(rows(), other.cols());
res.setZero();
// BT_PROFILE("btMatrixX mul");
for (int j=0; j < res.cols(); ++j)
// BT_PROFILE("btMatrixX mul");
for (int j = 0; j < res.cols(); ++j)
{
{
for (int i=0; i < res.rows(); ++i)
for (int i = 0; i < res.rows(); ++i)
{
T dotProd=0;
// T dotProd2=0;
T dotProd = 0;
// T dotProd2=0;
//int waste=0,waste2=0;
{
// bool useOtherCol = true;
// bool useOtherCol = true;
{
for (int v=0;v<rows();v++)
for (int v = 0; v < rows(); v++)
{
T w = (*this)(i,v);
if (other(v,j)!=0.f)
T w = (*this)(i, v);
if (other(v, j) != 0.f)
{
dotProd+=w*other(v,j);
dotProd += w * other(v, j);
}
}
}
}
if (dotProd)
res.setElem(i,j,dotProd);
res.setElem(i, j, dotProd);
}
}
}
@@ -381,174 +366,166 @@ struct btMatrixX
}
// this assumes the 4th and 8th rows of B and C are zero.
void multiplyAdd2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther ,int row, int col)
void multiplyAdd2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col)
{
const btScalar *bb = B;
for ( int i = 0;i<numRows;i++)
const btScalar* bb = B;
for (int i = 0; i < numRows; i++)
{
const btScalar *cc = C;
for ( int j = 0;j<numRowsOther;j++)
const btScalar* cc = C;
for (int j = 0; j < numRowsOther; j++)
{
btScalar sum;
sum = bb[0]*cc[0];
sum += bb[1]*cc[1];
sum += bb[2]*cc[2];
sum += bb[4]*cc[4];
sum += bb[5]*cc[5];
sum += bb[6]*cc[6];
addElem(row+i,col+j,sum);
sum = bb[0] * cc[0];
sum += bb[1] * cc[1];
sum += bb[2] * cc[2];
sum += bb[4] * cc[4];
sum += bb[5] * cc[5];
sum += bb[6] * cc[6];
addElem(row + i, col + j, sum);
cc += 8;
}
bb += 8;
}
}
void multiply2_p8r (const btScalar *B, const btScalar *C, int numRows, int numRowsOther, int row, int col)
void multiply2_p8r(const btScalar* B, const btScalar* C, int numRows, int numRowsOther, int row, int col)
{
btAssert (numRows>0 && numRowsOther>0 && B && C);
const btScalar *bb = B;
for ( int i = 0;i<numRows;i++)
btAssert(numRows > 0 && numRowsOther > 0 && B && C);
const btScalar* bb = B;
for (int i = 0; i < numRows; i++)
{
const btScalar *cc = C;
for ( int j = 0;j<numRowsOther;j++)
const btScalar* cc = C;
for (int j = 0; j < numRowsOther; j++)
{
btScalar sum;
sum = bb[0]*cc[0];
sum += bb[1]*cc[1];
sum += bb[2]*cc[2];
sum += bb[4]*cc[4];
sum += bb[5]*cc[5];
sum += bb[6]*cc[6];
setElem(row+i,col+j,sum);
sum = bb[0] * cc[0];
sum += bb[1] * cc[1];
sum += bb[2] * cc[2];
sum += bb[4] * cc[4];
sum += bb[5] * cc[5];
sum += bb[6] * cc[6];
setElem(row + i, col + j, sum);
cc += 8;
}
bb += 8;
}
}
void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const T value)
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const T value)
{
int numRows = rowend+1-rowstart;
int numCols = colend+1-colstart;
for (int row=0;row<numRows;row++)
int numRows = rowend + 1 - rowstart;
int numCols = colend + 1 - colstart;
for (int row = 0; row < numRows; row++)
{
for (int col=0;col<numCols;col++)
for (int col = 0; col < numCols; col++)
{
setElem(rowstart+row,colstart+col,value);
setElem(rowstart + row, colstart + col, value);
}
}
}
void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btMatrixX& block)
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btMatrixX& block)
{
btAssert(rowend+1-rowstart == block.rows());
btAssert(colend+1-colstart == block.cols());
for (int row=0;row<block.rows();row++)
btAssert(rowend + 1 - rowstart == block.rows());
btAssert(colend + 1 - colstart == block.cols());
for (int row = 0; row < block.rows(); row++)
{
for (int col=0;col<block.cols();col++)
for (int col = 0; col < block.cols(); col++)
{
setElem(rowstart+row,colstart+col,block(row,col));
setElem(rowstart + row, colstart + col, block(row, col));
}
}
}
void setSubMatrix(int rowstart,int colstart,int rowend,int colend,const btVectorX<T>& block)
void setSubMatrix(int rowstart, int colstart, int rowend, int colend, const btVectorX<T>& block)
{
btAssert(rowend+1-rowstart == block.rows());
btAssert(colend+1-colstart == block.cols());
for (int row=0;row<block.rows();row++)
btAssert(rowend + 1 - rowstart == block.rows());
btAssert(colend + 1 - colstart == block.cols());
for (int row = 0; row < block.rows(); row++)
{
for (int col=0;col<block.cols();col++)
for (int col = 0; col < block.cols(); col++)
{
setElem(rowstart+row,colstart+col,block[row]);
setElem(rowstart + row, colstart + col, block[row]);
}
}
}
btMatrixX negative()
{
btMatrixX neg(rows(),cols());
for (int i=0;i<rows();i++)
for (int j=0;j<cols();j++)
btMatrixX neg(rows(), cols());
for (int i = 0; i < rows(); i++)
for (int j = 0; j < cols(); j++)
{
T v = (*this)(i,j);
neg.setElem(i,j,-v);
T v = (*this)(i, j);
neg.setElem(i, j, -v);
}
return neg;
}
};
typedef btMatrixX<float> btMatrixXf;
typedef btVectorX<float> btVectorXf;
typedef btMatrixX<double> btMatrixXd;
typedef btVectorX<double> btVectorXd;
#ifdef BT_DEBUG_OSTREAM
template <typename T>
std::ostream& operator<< (std::ostream& os, const btMatrixX<T>& mat)
template <typename T>
std::ostream& operator<<(std::ostream& os, const btMatrixX<T>& mat)
{
os << " [";
//printf("%s ---------------------\n",msg);
for (int i = 0; i < mat.rows(); i++)
{
os << " [";
//printf("%s ---------------------\n",msg);
for (int i=0;i<mat.rows();i++)
for (int j = 0; j < mat.cols(); j++)
{
for (int j=0;j<mat.cols();j++)
{
os << std::setw(12) << mat(i,j);
}
if (i!=mat.rows()-1)
os << std::endl << " ";
os << std::setw(12) << mat(i, j);
}
os << " ]";
//printf("\n---------------------\n");
return os;
if (i != mat.rows() - 1)
os << std::endl
<< " ";
}
template <typename T>
std::ostream& operator<< (std::ostream& os, const btVectorX<T>& mat)
os << " ]";
//printf("\n---------------------\n");
return os;
}
template <typename T>
std::ostream& operator<<(std::ostream& os, const btVectorX<T>& mat)
{
os << " [";
//printf("%s ---------------------\n",msg);
for (int i = 0; i < mat.rows(); i++)
{
os << " [";
//printf("%s ---------------------\n",msg);
for (int i=0;i<mat.rows();i++)
{
os << std::setw(12) << mat[i];
if (i!=mat.rows()-1)
os << std::endl << " ";
}
os << " ]";
//printf("\n---------------------\n");
return os;
os << std::setw(12) << mat[i];
if (i != mat.rows() - 1)
os << std::endl
<< " ";
}
os << " ]";
//printf("\n---------------------\n");
#endif //BT_DEBUG_OSTREAM
return os;
}
#endif //BT_DEBUG_OSTREAM
inline void setElem(btMatrixXd& mat, int row, int col, double val)
{
mat.setElem(row,col,val);
mat.setElem(row, col, val);
}
inline void setElem(btMatrixXf& mat, int row, int col, float val)
{
mat.setElem(row,col,val);
mat.setElem(row, col, val);
}
#ifdef BT_USE_DOUBLE_PRECISION
#define btVectorXu btVectorXd
#define btMatrixXu btMatrixXd
#define btVectorXu btVectorXd
#define btMatrixXu btMatrixXd
#else
#define btVectorXu btVectorXf
#define btMatrixXu btMatrixXf
#endif //BT_USE_DOUBLE_PRECISION
#define btVectorXu btVectorXf
#define btMatrixXu btMatrixXf
#endif //BT_USE_DOUBLE_PRECISION
#endif//BT_MATRIX_H_H
#endif //BT_MATRIX_H_H

32
src/LinearMath/btMinMax.h
View File

@@ -12,60 +12,58 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_GEN_MINMAX_H
#define BT_GEN_MINMAX_H
#include "btScalar.h"
template <class T>
SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)
SIMD_FORCE_INLINE const T& btMin(const T& a, const T& b)
{
return a < b ? a : b ;
return a < b ? a : b;
}
template <class T>
SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b)
SIMD_FORCE_INLINE const T& btMax(const T& a, const T& b)
{
return a > b ? a : b;
return a > b ? a : b;
}
template <class T>
SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub)
SIMD_FORCE_INLINE const T& btClamped(const T& a, const T& lb, const T& ub)
{
return a < lb ? lb : (ub < a ? ub : a);
return a < lb ? lb : (ub < a ? ub : a);
}
template <class T>
SIMD_FORCE_INLINE void btSetMin(T& a, const T& b)
SIMD_FORCE_INLINE void btSetMin(T& a, const T& b)
{
if (b < a)
if (b < a)
{
a = b;
}
}
template <class T>
SIMD_FORCE_INLINE void btSetMax(T& a, const T& b)
SIMD_FORCE_INLINE void btSetMax(T& a, const T& b)
{
if (a < b)
if (a < b)
{
a = b;
}
}
template <class T>
SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub)
SIMD_FORCE_INLINE void btClamp(T& a, const T& lb, const T& ub)
{
if (a < lb)
if (a < lb)
{
a = lb;
a = lb;
}
else if (ub < a)
else if (ub < a)
{
a = ub;
}
}
#endif //BT_GEN_MINMAX_H
#endif //BT_GEN_MINMAX_H

24
src/LinearMath/btMotionState.h
View File

@@ -20,21 +20,17 @@ subject to the following restrictions:
///The btMotionState interface class allows the dynamics world to synchronize and interpolate the updated world transforms with graphics
///For optimizations, potentially only moving objects get synchronized (using setWorldPosition/setWorldOrientation)
class btMotionState
class btMotionState
{
public:
virtual ~btMotionState()
{
}
virtual void getWorldTransform(btTransform& worldTrans ) const =0;
public:
virtual ~btMotionState()
{
}
//Bullet only calls the update of worldtransform for active objects
virtual void setWorldTransform(const btTransform& worldTrans)=0;
virtual void getWorldTransform(btTransform& worldTrans) const = 0;
//Bullet only calls the update of worldtransform for active objects
virtual void setWorldTransform(const btTransform& worldTrans) = 0;
};
#endif //BT_MOTIONSTATE_H
#endif //BT_MOTIONSTATE_H

126
src/LinearMath/btPolarDecomposition.cpp
View File

@@ -3,96 +3,92 @@
namespace
{
btScalar abs_column_sum(const btMatrix3x3& a, int i)
{
return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]);
}
btScalar abs_row_sum(const btMatrix3x3& a, int i)
{
return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]);
}
btScalar p1_norm(const btMatrix3x3& a)
{
const btScalar sum0 = abs_column_sum(a,0);
const btScalar sum1 = abs_column_sum(a,1);
const btScalar sum2 = abs_column_sum(a,2);
return btMax(btMax(sum0, sum1), sum2);
}
btScalar pinf_norm(const btMatrix3x3& a)
{
const btScalar sum0 = abs_row_sum(a,0);
const btScalar sum1 = abs_row_sum(a,1);
const btScalar sum2 = abs_row_sum(a,2);
return btMax(btMax(sum0, sum1), sum2);
}
btScalar abs_column_sum(const btMatrix3x3& a, int i)
{
return btFabs(a[0][i]) + btFabs(a[1][i]) + btFabs(a[2][i]);
}
btScalar abs_row_sum(const btMatrix3x3& a, int i)
{
return btFabs(a[i][0]) + btFabs(a[i][1]) + btFabs(a[i][2]);
}
btScalar p1_norm(const btMatrix3x3& a)
{
const btScalar sum0 = abs_column_sum(a, 0);
const btScalar sum1 = abs_column_sum(a, 1);
const btScalar sum2 = abs_column_sum(a, 2);
return btMax(btMax(sum0, sum1), sum2);
}
btScalar pinf_norm(const btMatrix3x3& a)
{
const btScalar sum0 = abs_row_sum(a, 0);
const btScalar sum1 = abs_row_sum(a, 1);
const btScalar sum2 = abs_row_sum(a, 2);
return btMax(btMax(sum0, sum1), sum2);
}
} // namespace
btPolarDecomposition::btPolarDecomposition(btScalar tolerance, unsigned int maxIterations)
: m_tolerance(tolerance)
, m_maxIterations(maxIterations)
: m_tolerance(tolerance), m_maxIterations(maxIterations)
{
}
unsigned int btPolarDecomposition::decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const
{
// Use the 'u' and 'h' matrices for intermediate calculations
u = a;
h = a.inverse();
// Use the 'u' and 'h' matrices for intermediate calculations
u = a;
h = a.inverse();
for (unsigned int i = 0; i < m_maxIterations; ++i)
{
const btScalar h_1 = p1_norm(h);
const btScalar h_inf = pinf_norm(h);
const btScalar u_1 = p1_norm(u);
const btScalar u_inf = pinf_norm(u);
for (unsigned int i = 0; i < m_maxIterations; ++i)
{
const btScalar h_1 = p1_norm(h);
const btScalar h_inf = pinf_norm(h);
const btScalar u_1 = p1_norm(u);
const btScalar u_inf = pinf_norm(u);
const btScalar h_norm = h_1 * h_inf;
const btScalar u_norm = u_1 * u_inf;
const btScalar h_norm = h_1 * h_inf;
const btScalar u_norm = u_1 * u_inf;
// The matrix is effectively singular so we cannot invert it
if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
break;
// The matrix is effectively singular so we cannot invert it
if (btFuzzyZero(h_norm) || btFuzzyZero(u_norm))
break;
const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
const btScalar inv_gamma = btScalar(1.0) / gamma;
const btScalar gamma = btPow(h_norm / u_norm, 0.25f);
const btScalar inv_gamma = btScalar(1.0) / gamma;
// Determine the delta to 'u'
const btMatrix3x3 delta = (u * (gamma - btScalar(2.0)) + h.transpose() * inv_gamma) * btScalar(0.5);
// Determine the delta to 'u'
const btMatrix3x3 delta = (u * (gamma - btScalar(2.0)) + h.transpose() * inv_gamma) * btScalar(0.5);
// Update the matrices
u += delta;
h = u.inverse();
// Update the matrices
u += delta;
h = u.inverse();
// Check for convergence
if (p1_norm(delta) <= m_tolerance * u_1)
{
h = u.transpose() * a;
h = (h + h.transpose()) * 0.5;
return i;
}
}
// Check for convergence
if (p1_norm(delta) <= m_tolerance * u_1)
{
h = u.transpose() * a;
h = (h + h.transpose()) * 0.5;
return i;
}
}
// The algorithm has failed to converge to the specified tolerance, but we
// want to make sure that the matrices returned are in the right form.
h = u.transpose() * a;
h = (h + h.transpose()) * 0.5;
// The algorithm has failed to converge to the specified tolerance, but we
// want to make sure that the matrices returned are in the right form.
h = u.transpose() * a;
h = (h + h.transpose()) * 0.5;
return m_maxIterations;
return m_maxIterations;
}
unsigned int btPolarDecomposition::maxIterations() const
{
return m_maxIterations;
return m_maxIterations;
}
unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h)
{
static btPolarDecomposition polar;
return polar.decompose(a, u, h);
static btPolarDecomposition polar;
return polar.decompose(a, u, h);
}

29
src/LinearMath/btPolarDecomposition.h
View File

@@ -13,10 +13,8 @@
*/
class btPolarDecomposition
{
public:
/**
public:
/**
* Creates an instance with optional parameters.
*
* @param tolerance - the tolerance used to determine convergence of the
@@ -24,10 +22,10 @@ class btPolarDecomposition
* @param maxIterations - the maximum number of iterations used to achieve
* convergence
*/
btPolarDecomposition(btScalar tolerance = btScalar(0.0001),
unsigned int maxIterations = 16);
btPolarDecomposition(btScalar tolerance = btScalar(0.0001),
unsigned int maxIterations = 16);
/**
/**
* Decomposes a matrix into orthogonal and symmetric, positive-definite
* parts. If the number of iterations returned by this function is equal to
* the maximum number of iterations, the algorithm has failed to converge.
@@ -38,19 +36,19 @@ class btPolarDecomposition
*
* @return the number of iterations performed by the algorithm.
*/
unsigned int decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const;
unsigned int decompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h) const;
/**
/**
* Returns the maximum number of iterations that this algorithm will perform
* to achieve convergence.
*
* @return maximum number of iterations
*/
unsigned int maxIterations() const;
unsigned int maxIterations() const;
private:
btScalar m_tolerance;
unsigned int m_maxIterations;
private:
btScalar m_tolerance;
unsigned int m_maxIterations;
};
/**
@@ -66,7 +64,6 @@ class btPolarDecomposition
*
* @return the number of iterations performed by the algorithm.
*/
unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h);
#endif // POLARDECOMPOSITION_H
unsigned int polarDecompose(const btMatrix3x3& a, btMatrix3x3& u, btMatrix3x3& h);
#endif // POLARDECOMPOSITION_H

96
src/LinearMath/btPoolAllocator.h
View File

@@ -12,7 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef _BT_POOL_ALLOCATOR_H
#define _BT_POOL_ALLOCATOR_H
@@ -23,38 +22,38 @@ subject to the following restrictions:
///The btPoolAllocator class allows to efficiently allocate a large pool of objects, instead of dynamically allocating them separately.
class btPoolAllocator
{
int m_elemSize;
int m_maxElements;
int m_freeCount;
void* m_firstFree;
unsigned char* m_pool;
btSpinMutex m_mutex; // only used if BT_THREADSAFE
int m_elemSize;
int m_maxElements;
int m_freeCount;
void* m_firstFree;
unsigned char* m_pool;
btSpinMutex m_mutex; // only used if BT_THREADSAFE
public:
btPoolAllocator(int elemSize, int maxElements)
:m_elemSize(elemSize),
m_maxElements(maxElements)
: m_elemSize(elemSize),
m_maxElements(maxElements)
{
m_pool = (unsigned char*) btAlignedAlloc( static_cast<unsigned int>(m_elemSize*m_maxElements),16);
m_pool = (unsigned char*)btAlignedAlloc(static_cast<unsigned int>(m_elemSize * m_maxElements), 16);
unsigned char* p = m_pool;
m_firstFree = p;
m_freeCount = m_maxElements;
int count = m_maxElements;
while (--count) {
*(void**)p = (p + m_elemSize);
p += m_elemSize;
}
*(void**)p = 0;
}
m_firstFree = p;
m_freeCount = m_maxElements;
int count = m_maxElements;
while (--count)
{
*(void**)p = (p + m_elemSize);
p += m_elemSize;
}
*(void**)p = 0;
}
~btPoolAllocator()
{
btAlignedFree( m_pool);
btAlignedFree(m_pool);
}
int getFreeCount() const
int getFreeCount() const
{
return m_freeCount;
}
@@ -69,26 +68,27 @@ public:
return m_maxElements;
}
void* allocate(int size)
void* allocate(int size)
{
// release mode fix
(void)size;
btMutexLock(&m_mutex);
btAssert(!size || size<=m_elemSize);
btMutexLock(&m_mutex);
btAssert(!size || size <= m_elemSize);
//btAssert(m_freeCount>0); // should return null if all full
void* result = m_firstFree;
if (NULL != m_firstFree)
{
m_firstFree = *(void**)m_firstFree;
--m_freeCount;
}
btMutexUnlock(&m_mutex);
return result;
void* result = m_firstFree;
if (NULL != m_firstFree)
{
m_firstFree = *(void**)m_firstFree;
--m_freeCount;
}
btMutexUnlock(&m_mutex);
return result;
}
bool validPtr(void* ptr)
{
if (ptr) {
if (ptr)
{
if (((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize))
{
return true;
@@ -97,34 +97,34 @@ public:
return false;
}
void freeMemory(void* ptr)
void freeMemory(void* ptr)
{
if (ptr) {
btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
if (ptr)
{
btAssert((unsigned char*)ptr >= m_pool && (unsigned char*)ptr < m_pool + m_maxElements * m_elemSize);
btMutexLock(&m_mutex);
*(void**)ptr = m_firstFree;
m_firstFree = ptr;
++m_freeCount;
btMutexUnlock(&m_mutex);
}
btMutexLock(&m_mutex);
*(void**)ptr = m_firstFree;
m_firstFree = ptr;
++m_freeCount;
btMutexUnlock(&m_mutex);
}
}
int getElementSize() const
int getElementSize() const
{
return m_elemSize;
}
unsigned char* getPoolAddress()
unsigned char* getPoolAddress()
{
return m_pool;
}
const unsigned char* getPoolAddress() const
const unsigned char* getPoolAddress() const
{
return m_pool;
}
};
#endif //_BT_POOL_ALLOCATOR_H
#endif //_BT_POOL_ALLOCATOR_H

244
src/LinearMath/btQuadWord.h
View File

@@ -12,18 +12,13 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_SIMD_QUADWORD_H
#define BT_SIMD_QUADWORD_H
#include "btScalar.h"
#include "btMinMax.h"
#if defined (__CELLOS_LV2) && defined (__SPU__)
#if defined(__CELLOS_LV2) && defined(__SPU__)
#include <altivec.h>
#endif
@@ -31,51 +26,53 @@ subject to the following restrictions:
* Some issues under PS3 Linux with IBM 2.1 SDK, gcc compiler prevent from using aligned quadword.
*/
#ifndef USE_LIBSPE2
ATTRIBUTE_ALIGNED16(class) btQuadWord
ATTRIBUTE_ALIGNED16(class)
btQuadWord
#else
class btQuadWord
#endif
{
protected:
#if defined (__SPU__) && defined (__CELLOS_LV2__)
#if defined(__SPU__) && defined(__CELLOS_LV2__)
union {
vec_float4 mVec128;
btScalar m_floats[4];
btScalar m_floats[4];
};
public:
vec_float4 get128() const
vec_float4 get128() const
{
return mVec128;
}
protected:
#else //__CELLOS_LV2__ __SPU__
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
protected:
#else //__CELLOS_LV2__ __SPU__
#if defined(BT_USE_SSE) || defined(BT_USE_NEON)
union {
btSimdFloat4 mVec128;
btScalar m_floats[4];
btScalar m_floats[4];
};
public:
SIMD_FORCE_INLINE btSimdFloat4 get128() const
SIMD_FORCE_INLINE btSimdFloat4 get128() const
{
return mVec128;
}
SIMD_FORCE_INLINE void set128(btSimdFloat4 v128)
SIMD_FORCE_INLINE void set128(btSimdFloat4 v128)
{
mVec128 = v128;
}
#else
btScalar m_floats[4];
#endif // BT_USE_SSE
btScalar m_floats[4];
#endif // BT_USE_SSE
#endif //__CELLOS_LV2__ __SPU__
#endif //__CELLOS_LV2__ __SPU__
public:
public:
#if (defined(BT_USE_SSE_IN_API) && defined(BT_USE_SSE)) || defined(BT_USE_NEON)
// Set Vector
// Set Vector
SIMD_FORCE_INLINE btQuadWord(const btSimdFloat4 vec)
{
mVec128 = vec;
@@ -88,157 +85,154 @@ public:
}
// Assignment Operator
SIMD_FORCE_INLINE btQuadWord&
operator=(const btQuadWord& v)
SIMD_FORCE_INLINE btQuadWord&
operator=(const btQuadWord& v)
{
mVec128 = v.mVec128;
return *this;
}
#endif
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x;};
/**@brief Set the y value */
SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y;};
/**@brief Set the z value */
SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z;};
/**@brief Set the w value */
SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w;};
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
/**@brief Return the y value */
SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
/**@brief Return the w value */
SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& getX() const { return m_floats[0]; }
/**@brief Return the y value */
SIMD_FORCE_INLINE const btScalar& getY() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& getZ() const { return m_floats[2]; }
/**@brief Set the x value */
SIMD_FORCE_INLINE void setX(btScalar _x) { m_floats[0] = _x; };
/**@brief Set the y value */
SIMD_FORCE_INLINE void setY(btScalar _y) { m_floats[1] = _y; };
/**@brief Set the z value */
SIMD_FORCE_INLINE void setZ(btScalar _z) { m_floats[2] = _z; };
/**@brief Set the w value */
SIMD_FORCE_INLINE void setW(btScalar _w) { m_floats[3] = _w; };
/**@brief Return the x value */
SIMD_FORCE_INLINE const btScalar& x() const { return m_floats[0]; }
/**@brief Return the y value */
SIMD_FORCE_INLINE const btScalar& y() const { return m_floats[1]; }
/**@brief Return the z value */
SIMD_FORCE_INLINE const btScalar& z() const { return m_floats[2]; }
/**@brief Return the w value */
SIMD_FORCE_INLINE const btScalar& w() const { return m_floats[3]; }
//SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
//SIMD_FORCE_INLINE btScalar& operator[](int i) { return (&m_floats[0])[i]; }
//SIMD_FORCE_INLINE const btScalar& operator[](int i) const { return (&m_floats[0])[i]; }
///operator btScalar*() replaces operator[], using implicit conversion. We added operator != and operator == to avoid pointer comparisons.
SIMD_FORCE_INLINE operator btScalar *() { return &m_floats[0]; }
SIMD_FORCE_INLINE operator const btScalar *() const { return &m_floats[0]; }
SIMD_FORCE_INLINE operator btScalar*() { return &m_floats[0]; }
SIMD_FORCE_INLINE operator const btScalar*() const { return &m_floats[0]; }
SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const
SIMD_FORCE_INLINE bool operator==(const btQuadWord& other) const
{
#ifdef BT_USE_SSE
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
#else
return ((m_floats[3]==other.m_floats[3]) &&
(m_floats[2]==other.m_floats[2]) &&
(m_floats[1]==other.m_floats[1]) &&
(m_floats[0]==other.m_floats[0]));
return (0xf == _mm_movemask_ps((__m128)_mm_cmpeq_ps(mVec128, other.mVec128)));
#else
return ((m_floats[3] == other.m_floats[3]) &&
(m_floats[2] == other.m_floats[2]) &&
(m_floats[1] == other.m_floats[1]) &&
(m_floats[0] == other.m_floats[0]));
#endif
}
SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const
SIMD_FORCE_INLINE bool operator!=(const btQuadWord& other) const
{
return !(*this == other);
}
/**@brief Set x,y,z and zero w
/**@brief Set x,y,z and zero w
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
{
m_floats[0]=_x;
m_floats[1]=_y;
m_floats[2]=_z;
m_floats[3] = 0.f;
}
SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z)
{
m_floats[0] = _x;
m_floats[1] = _y;
m_floats[2] = _z;
m_floats[3] = 0.f;
}
/* void getValue(btScalar *m) const
/* void getValue(btScalar *m) const
{
m[0] = m_floats[0];
m[1] = m_floats[1];
m[2] = m_floats[2];
}
*/
/**@brief Set the values
/**@brief Set the values
* @param x Value of x
* @param y Value of y
* @param z Value of z
* @param w Value of w
*/
SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
{
m_floats[0]=_x;
m_floats[1]=_y;
m_floats[2]=_z;
m_floats[3]=_w;
}
/**@brief No initialization constructor */
SIMD_FORCE_INLINE btQuadWord()
// :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
{
}
/**@brief Three argument constructor (zeros w)
SIMD_FORCE_INLINE void setValue(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
{
m_floats[0] = _x;
m_floats[1] = _y;
m_floats[2] = _z;
m_floats[3] = _w;
}
/**@brief No initialization constructor */
SIMD_FORCE_INLINE btQuadWord()
// :m_floats[0](btScalar(0.)),m_floats[1](btScalar(0.)),m_floats[2](btScalar(0.)),m_floats[3](btScalar(0.))
{
}
/**@brief Three argument constructor (zeros w)
* @param x Value of x
* @param y Value of y
* @param z Value of z
*/
SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z)
{
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
}
SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z)
{
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = 0.0f;
}
/**@brief Initializing constructor
/**@brief Initializing constructor
* @param x Value of x
* @param y Value of y
* @param z Value of z
* @param w Value of w
*/
SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z,const btScalar& _w)
{
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
}
SIMD_FORCE_INLINE btQuadWord(const btScalar& _x, const btScalar& _y, const btScalar& _z, const btScalar& _w)
{
m_floats[0] = _x, m_floats[1] = _y, m_floats[2] = _z, m_floats[3] = _w;
}
/**@brief Set each element to the max of the current values and the values of another btQuadWord
/**@brief Set each element to the max of the current values and the values of another btQuadWord
* @param other The other btQuadWord to compare with
*/
SIMD_FORCE_INLINE void setMax(const btQuadWord& other)
{
#ifdef BT_USE_SSE
mVec128 = _mm_max_ps(mVec128, other.mVec128);
#elif defined(BT_USE_NEON)
mVec128 = vmaxq_f32(mVec128, other.mVec128);
#else
btSetMax(m_floats[0], other.m_floats[0]);
btSetMax(m_floats[1], other.m_floats[1]);
btSetMax(m_floats[2], other.m_floats[2]);
btSetMax(m_floats[3], other.m_floats[3]);
#endif
}
/**@brief Set each element to the min of the current values and the values of another btQuadWord
SIMD_FORCE_INLINE void setMax(const btQuadWord& other)
{
#ifdef BT_USE_SSE
mVec128 = _mm_max_ps(mVec128, other.mVec128);
#elif defined(BT_USE_NEON)
mVec128 = vmaxq_f32(mVec128, other.mVec128);
#else
btSetMax(m_floats[0], other.m_floats[0]);
btSetMax(m_floats[1], other.m_floats[1]);
btSetMax(m_floats[2], other.m_floats[2]);
btSetMax(m_floats[3], other.m_floats[3]);
#endif
}
/**@brief Set each element to the min of the current values and the values of another btQuadWord
* @param other The other btQuadWord to compare with
*/
SIMD_FORCE_INLINE void setMin(const btQuadWord& other)
{
#ifdef BT_USE_SSE
mVec128 = _mm_min_ps(mVec128, other.mVec128);
#elif defined(BT_USE_NEON)
mVec128 = vminq_f32(mVec128, other.mVec128);
#else
btSetMin(m_floats[0], other.m_floats[0]);
btSetMin(m_floats[1], other.m_floats[1]);
btSetMin(m_floats[2], other.m_floats[2]);
btSetMin(m_floats[3], other.m_floats[3]);
#endif
}
SIMD_FORCE_INLINE void setMin(const btQuadWord& other)
{
#ifdef BT_USE_SSE
mVec128 = _mm_min_ps(mVec128, other.mVec128);
#elif defined(BT_USE_NEON)
mVec128 = vminq_f32(mVec128, other.mVec128);
#else
btSetMin(m_floats[0], other.m_floats[0]);
btSetMin(m_floats[1], other.m_floats[1]);
btSetMin(m_floats[2], other.m_floats[2]);
btSetMin(m_floats[3], other.m_floats[3]);
#endif
}
};
#endif //BT_SIMD_QUADWORD_H
#endif //BT_SIMD_QUADWORD_H

869
src/LinearMath/btQuaternion.h
View File

File diff suppressed because it is too large Load Diff

575
src/LinearMath/btQuickprof.cpp
View File

@@ -16,16 +16,13 @@
#include "btQuickprof.h"
#include "btThreads.h"
#ifdef __CELLOS_LV2__
#include <sys/sys_time.h>
#include <sys/time_util.h>
#include <stdio.h>
#endif
#if defined (SUNOS) || defined (__SUNOS__)
#if defined(SUNOS) || defined(__SUNOS__)
#include <stdio.h>
#endif
#ifdef __APPLE__
@@ -42,49 +39,46 @@
#define NOIME
#ifdef _XBOX
#include <Xtl.h>
#else //_XBOX
#include <windows.h>
#include <Xtl.h>
#else //_XBOX
#include <windows.h>
#if WINVER <0x0602
#if WINVER < 0x0602
#define GetTickCount64 GetTickCount
#endif
#endif //_XBOX
#endif //_XBOX
#include <time.h>
#else //_WIN32
#else //_WIN32
#include <sys/time.h>
#ifdef BT_LINUX_REALTIME
//required linking against rt (librt)
#include <time.h>
#endif //BT_LINUX_REALTIME
#endif //BT_LINUX_REALTIME
#endif //_WIN32
#endif //_WIN32
#define mymin(a,b) (a > b ? a : b)
#define mymin(a, b) (a > b ? a : b)
struct btClockData
{
#ifdef BT_USE_WINDOWS_TIMERS
LARGE_INTEGER mClockFrequency;
LONGLONG mStartTick;
LARGE_INTEGER mStartTime;
#else
#ifdef __CELLOS_LV2__
uint64_t mStartTime;
uint64_t mStartTime;
#else
#ifdef __APPLE__
uint64_t mStartTimeNano;
uint64_t mStartTimeNano;
#endif
struct timeval mStartTime;
#endif
#endif //__CELLOS_LV2__
#endif //__CELLOS_LV2__
};
///The btClock is a portable basic clock that measures accurate time in seconds, use for profiling.
@@ -114,8 +108,7 @@ btClock& btClock::operator=(const btClock& other)
return *this;
}
/// Resets the initial reference time.
/// Resets the initial reference time.
void btClock::reset()
{
#ifdef BT_USE_WINDOWS_TIMERS
@@ -124,14 +117,14 @@ void btClock::reset()
#else
#ifdef __CELLOS_LV2__
typedef uint64_t ClockSize;
typedef uint64_t ClockSize;
ClockSize newTime;
//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
SYS_TIMEBASE_GET( newTime );
SYS_TIMEBASE_GET(newTime);
m_data->mStartTime = newTime;
#else
#ifdef __APPLE__
m_data->mStartTimeNano = mach_absolute_time();
m_data->mStartTimeNano = mach_absolute_time();
#endif
gettimeofday(&m_data->mStartTime, 0);
#endif
@@ -146,66 +139,66 @@ unsigned long long int btClock::getTimeMilliseconds()
LARGE_INTEGER currentTime;
QueryPerformanceCounter(&currentTime);
LONGLONG elapsedTime = currentTime.QuadPart -
m_data->mStartTime.QuadPart;
// Compute the number of millisecond ticks elapsed.
m_data->mStartTime.QuadPart;
// Compute the number of millisecond ticks elapsed.
unsigned long msecTicks = (unsigned long)(1000 * elapsedTime /
m_data->mClockFrequency.QuadPart);
m_data->mClockFrequency.QuadPart);
return msecTicks;
return msecTicks;
#else
#ifdef __CELLOS_LV2__
uint64_t freq=sys_time_get_timebase_frequency();
double dFreq=((double) freq) / 1000.0;
typedef uint64_t ClockSize;
ClockSize newTime;
SYS_TIMEBASE_GET( newTime );
//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
uint64_t freq = sys_time_get_timebase_frequency();
double dFreq = ((double)freq) / 1000.0;
typedef uint64_t ClockSize;
ClockSize newTime;
SYS_TIMEBASE_GET(newTime);
//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
return (unsigned long int)((double(newTime - m_data->mStartTime)) / dFreq);
#else
struct timeval currentTime;
gettimeofday(&currentTime, 0);
return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 +
(currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000;
#endif //__CELLOS_LV2__
struct timeval currentTime;
gettimeofday(&currentTime, 0);
return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000 +
(currentTime.tv_usec - m_data->mStartTime.tv_usec) / 1000;
#endif //__CELLOS_LV2__
#endif
}
/// Returns the time in us since the last call to reset or since
/// the Clock was created.
/// Returns the time in us since the last call to reset or since
/// the Clock was created.
unsigned long long int btClock::getTimeMicroseconds()
{
#ifdef BT_USE_WINDOWS_TIMERS
//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx
LARGE_INTEGER currentTime, elapsedTime;
QueryPerformanceCounter(&currentTime);
elapsedTime.QuadPart = currentTime.QuadPart -
m_data->mStartTime.QuadPart;
elapsedTime.QuadPart *= 1000000;
elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx
LARGE_INTEGER currentTime, elapsedTime;
return (unsigned long long) elapsedTime.QuadPart;
QueryPerformanceCounter(&currentTime);
elapsedTime.QuadPart = currentTime.QuadPart -
m_data->mStartTime.QuadPart;
elapsedTime.QuadPart *= 1000000;
elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
return (unsigned long long)elapsedTime.QuadPart;
#else
#ifdef __CELLOS_LV2__
uint64_t freq=sys_time_get_timebase_frequency();
double dFreq=((double) freq)/ 1000000.0;
typedef uint64_t ClockSize;
ClockSize newTime;
//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
SYS_TIMEBASE_GET( newTime );
uint64_t freq = sys_time_get_timebase_frequency();
double dFreq = ((double)freq) / 1000000.0;
typedef uint64_t ClockSize;
ClockSize newTime;
//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
SYS_TIMEBASE_GET(newTime);
return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
return (unsigned long int)((double(newTime - m_data->mStartTime)) / dFreq);
#else
struct timeval currentTime;
gettimeofday(&currentTime, 0);
return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 +
(currentTime.tv_usec - m_data->mStartTime.tv_usec);
#endif//__CELLOS_LV2__
struct timeval currentTime;
gettimeofday(&currentTime, 0);
return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1000000 +
(currentTime.tv_usec - m_data->mStartTime.tv_usec);
#endif //__CELLOS_LV2__
#endif
}
@@ -213,65 +206,63 @@ unsigned long long int btClock::getTimeNanoseconds()
{
#ifdef BT_USE_WINDOWS_TIMERS
//see https://msdn.microsoft.com/en-us/library/windows/desktop/dn553408(v=vs.85).aspx
LARGE_INTEGER currentTime, elapsedTime;
QueryPerformanceCounter(&currentTime);
elapsedTime.QuadPart = currentTime.QuadPart -
m_data->mStartTime.QuadPart;
elapsedTime.QuadPart *= 1000000000;
elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
LARGE_INTEGER currentTime, elapsedTime;
return (unsigned long long) elapsedTime.QuadPart;
QueryPerformanceCounter(&currentTime);
elapsedTime.QuadPart = currentTime.QuadPart -
m_data->mStartTime.QuadPart;
elapsedTime.QuadPart *= 1000000000;
elapsedTime.QuadPart /= m_data->mClockFrequency.QuadPart;
return (unsigned long long)elapsedTime.QuadPart;
#else
#ifdef __CELLOS_LV2__
uint64_t freq=sys_time_get_timebase_frequency();
double dFreq=((double) freq)/ 1e9;
typedef uint64_t ClockSize;
ClockSize newTime;
//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
SYS_TIMEBASE_GET( newTime );
uint64_t freq = sys_time_get_timebase_frequency();
double dFreq = ((double)freq) / 1e9;
typedef uint64_t ClockSize;
ClockSize newTime;
//__asm __volatile__( "mftb %0" : "=r" (newTime) : : "memory");
SYS_TIMEBASE_GET(newTime);
return (unsigned long int)((double(newTime-m_data->mStartTime)) / dFreq);
return (unsigned long int)((double(newTime - m_data->mStartTime)) / dFreq);
#else
#ifdef __APPLE__
uint64_t ticks = mach_absolute_time() - m_data->mStartTimeNano;
static long double conversion = 0.0L;
if( 0.0L == conversion )
{
// attempt to get conversion to nanoseconds
mach_timebase_info_data_t info;
int err = mach_timebase_info( &info );
if( err )
{
btAssert(0);
conversion = 1.;
}
conversion = info.numer / info.denom;
}
return (ticks * conversion);
uint64_t ticks = mach_absolute_time() - m_data->mStartTimeNano;
static long double conversion = 0.0L;
if (0.0L == conversion)
{
// attempt to get conversion to nanoseconds
mach_timebase_info_data_t info;
int err = mach_timebase_info(&info);
if (err)
{
btAssert(0);
conversion = 1.;
}
conversion = info.numer / info.denom;
}
return (ticks * conversion);
#else //__APPLE__
#else//__APPLE__
#ifdef BT_LINUX_REALTIME
timespec ts;
clock_gettime(CLOCK_REALTIME,&ts);
return 1000000000*ts.tv_sec + ts.tv_nsec;
timespec ts;
clock_gettime(CLOCK_REALTIME, &ts);
return 1000000000 * ts.tv_sec + ts.tv_nsec;
#else
struct timeval currentTime;
gettimeofday(&currentTime, 0);
return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1e9 +
(currentTime.tv_usec - m_data->mStartTime.tv_usec)*1000;
#endif //BT_LINUX_REALTIME
struct timeval currentTime;
gettimeofday(&currentTime, 0);
return (currentTime.tv_sec - m_data->mStartTime.tv_sec) * 1e9 +
(currentTime.tv_usec - m_data->mStartTime.tv_usec) * 1000;
#endif //BT_LINUX_REALTIME
#endif//__APPLE__
#endif//__CELLOS_LV2__
#endif
#endif //__APPLE__
#endif //__CELLOS_LV2__
#endif
}
/// Returns the time in s since the last call to reset or since
/// Returns the time in s since the last call to reset or since
/// the Clock was created.
btScalar btClock::getTimeSeconds()
{
@@ -281,23 +272,19 @@ btScalar btClock::getTimeSeconds()
#ifndef BT_NO_PROFILE
static btClock gProfileClock;
inline void Profile_Get_Ticks(unsigned long int * ticks)
inline void Profile_Get_Ticks(unsigned long int* ticks)
{
*ticks = (unsigned long int)gProfileClock.getTimeMicroseconds();
}
inline float Profile_Get_Tick_Rate(void)
{
// return 1000000.f;
// return 1000000.f;
return 1000.f;
}
/***************************************************************************************************
**
** CProfileNode
@@ -313,35 +300,32 @@ inline float Profile_Get_Tick_Rate(void)
* The name is assumed to be a static pointer, only the pointer is stored and compared for *
* efficiency reasons. *
*=============================================================================================*/
CProfileNode::CProfileNode( const char * name, CProfileNode * parent ) :
Name( name ),
TotalCalls( 0 ),
TotalTime( 0 ),
StartTime( 0 ),
RecursionCounter( 0 ),
Parent( parent ),
Child( NULL ),
Sibling( NULL ),
m_userPtr(0)
CProfileNode::CProfileNode(const char* name, CProfileNode* parent) : Name(name),
TotalCalls(0),
TotalTime(0),
StartTime(0),
RecursionCounter(0),
Parent(parent),
Child(NULL),
Sibling(NULL),
m_userPtr(0)
{
Reset();
}
void CProfileNode::CleanupMemory()
void CProfileNode::CleanupMemory()
{
delete ( Child);
delete (Child);
Child = NULL;
delete ( Sibling);
delete (Sibling);
Sibling = NULL;
}
CProfileNode::~CProfileNode( void )
CProfileNode::~CProfileNode(void)
{
CleanupMemory();
}
/***********************************************************************************************
* INPUT: *
* name - static string pointer to the name of the node we are searching for *
@@ -350,12 +334,14 @@ CProfileNode::~CProfileNode( void )
* All profile names are assumed to be static strings so this function uses pointer compares *
* to find the named node. *
*=============================================================================================*/
CProfileNode * CProfileNode::Get_Sub_Node( const char * name )
CProfileNode* CProfileNode::Get_Sub_Node(const char* name)
{
// Try to find this sub node
CProfileNode * child = Child;
while ( child ) {
if ( child->Name == name ) {
CProfileNode* child = Child;
while (child)
{
if (child->Name == name)
{
return child;
}
child = child->Sibling;
@@ -363,176 +349,212 @@ CProfileNode * CProfileNode::Get_Sub_Node( const char * name )
// We didn't find it, so add it
CProfileNode * node = new CProfileNode( name, this );
CProfileNode* node = new CProfileNode(name, this);
node->Sibling = Child;
Child = node;
return node;
}
void CProfileNode::Reset( void )
void CProfileNode::Reset(void)
{
TotalCalls = 0;
TotalTime = 0.0f;
if ( Child ) {
if (Child)
{
Child->Reset();
}
if ( Sibling ) {
if (Sibling)
{
Sibling->Reset();
}
}
void CProfileNode::Call( void )
void CProfileNode::Call(void)
{
TotalCalls++;
if (RecursionCounter++ == 0) {
if (RecursionCounter++ == 0)
{
Profile_Get_Ticks(&StartTime);
}
}
bool CProfileNode::Return( void )
bool CProfileNode::Return(void)
{
if ( --RecursionCounter == 0 && TotalCalls != 0 ) {
if (--RecursionCounter == 0 && TotalCalls != 0)
{
unsigned long int time;
Profile_Get_Ticks(&time);
time-=StartTime;
time -= StartTime;
TotalTime += (float)time / Profile_Get_Tick_Rate();
}
return ( RecursionCounter == 0 );
return (RecursionCounter == 0);
}
/***************************************************************************************************
**
** CProfileIterator
**
***************************************************************************************************/
CProfileIterator::CProfileIterator( CProfileNode * start )
CProfileIterator::CProfileIterator(CProfileNode* start)
{
CurrentParent = start;
CurrentChild = CurrentParent->Get_Child();
}
void CProfileIterator::First(void)
void CProfileIterator::First(void)
{
CurrentChild = CurrentParent->Get_Child();
}
void CProfileIterator::Next(void)
void CProfileIterator::Next(void)
{
CurrentChild = CurrentChild->Get_Sibling();
}
bool CProfileIterator::Is_Done(void)
bool CProfileIterator::Is_Done(void)
{
return CurrentChild == NULL;
}
void CProfileIterator::Enter_Child( int index )
void CProfileIterator::Enter_Child(int index)
{
CurrentChild = CurrentParent->Get_Child();
while ( (CurrentChild != NULL) && (index != 0) ) {
while ((CurrentChild != NULL) && (index != 0))
{
index--;
CurrentChild = CurrentChild->Get_Sibling();
}
if ( CurrentChild != NULL ) {
if (CurrentChild != NULL)
{
CurrentParent = CurrentChild;
CurrentChild = CurrentParent->Get_Child();
}
}
void CProfileIterator::Enter_Parent( void )
void CProfileIterator::Enter_Parent(void)
{
if ( CurrentParent->Get_Parent() != NULL ) {
if (CurrentParent->Get_Parent() != NULL)
{
CurrentParent = CurrentParent->Get_Parent();
}
CurrentChild = CurrentParent->Get_Child();
}
/***************************************************************************************************
**
** CProfileManager
**
***************************************************************************************************/
CProfileNode gRoots[BT_QUICKPROF_MAX_THREAD_COUNT] = {
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL),
CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL), CProfileNode("Root", NULL)};
CProfileNode gRoots[BT_QUICKPROF_MAX_THREAD_COUNT]={
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),
CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL),CProfileNode("Root",NULL)
CProfileNode* gCurrentNodes[BT_QUICKPROF_MAX_THREAD_COUNT] =
{
&gRoots[0],
&gRoots[1],
&gRoots[2],
&gRoots[3],
&gRoots[4],
&gRoots[5],
&gRoots[6],
&gRoots[7],
&gRoots[8],
&gRoots[9],
&gRoots[10],
&gRoots[11],
&gRoots[12],
&gRoots[13],
&gRoots[14],
&gRoots[15],
&gRoots[16],
&gRoots[17],
&gRoots[18],
&gRoots[19],
&gRoots[20],
&gRoots[21],
&gRoots[22],
&gRoots[23],
&gRoots[24],
&gRoots[25],
&gRoots[26],
&gRoots[27],
&gRoots[28],
&gRoots[29],
&gRoots[30],
&gRoots[31],
&gRoots[32],
&gRoots[33],
&gRoots[34],
&gRoots[35],
&gRoots[36],
&gRoots[37],
&gRoots[38],
&gRoots[39],
&gRoots[40],
&gRoots[41],
&gRoots[42],
&gRoots[43],
&gRoots[44],
&gRoots[45],
&gRoots[46],
&gRoots[47],
&gRoots[48],
&gRoots[49],
&gRoots[50],
&gRoots[51],
&gRoots[52],
&gRoots[53],
&gRoots[54],
&gRoots[55],
&gRoots[56],
&gRoots[57],
&gRoots[58],
&gRoots[59],
&gRoots[60],
&gRoots[61],
&gRoots[62],
&gRoots[63],
};
int CProfileManager::FrameCounter = 0;
unsigned long int CProfileManager::ResetTime = 0;
CProfileNode* gCurrentNodes[BT_QUICKPROF_MAX_THREAD_COUNT]=
CProfileIterator* CProfileManager::Get_Iterator(void)
{
&gRoots[ 0], &gRoots[ 1], &gRoots[ 2], &gRoots[ 3],
&gRoots[ 4], &gRoots[ 5], &gRoots[ 6], &gRoots[ 7],
&gRoots[ 8], &gRoots[ 9], &gRoots[10], &gRoots[11],
&gRoots[12], &gRoots[13], &gRoots[14], &gRoots[15],
&gRoots[16], &gRoots[17], &gRoots[18], &gRoots[19],
&gRoots[20], &gRoots[21], &gRoots[22], &gRoots[23],
&gRoots[24], &gRoots[25], &gRoots[26], &gRoots[27],
&gRoots[28], &gRoots[29], &gRoots[30], &gRoots[31],
&gRoots[32], &gRoots[33], &gRoots[34], &gRoots[35],
&gRoots[36], &gRoots[37], &gRoots[38], &gRoots[39],
&gRoots[40], &gRoots[41], &gRoots[42], &gRoots[43],
&gRoots[44], &gRoots[45], &gRoots[46], &gRoots[47],
&gRoots[48], &gRoots[49], &gRoots[50], &gRoots[51],
&gRoots[52], &gRoots[53], &gRoots[54], &gRoots[55],
&gRoots[56], &gRoots[57], &gRoots[58], &gRoots[59],
&gRoots[60], &gRoots[61], &gRoots[62], &gRoots[63],
};
int threadIndex = btQuickprofGetCurrentThreadIndex2();
if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
return 0;
int CProfileManager::FrameCounter = 0;
unsigned long int CProfileManager::ResetTime = 0;
CProfileIterator * CProfileManager::Get_Iterator( void )
{
int threadIndex = btQuickprofGetCurrentThreadIndex2();
if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
return 0;
return new CProfileIterator( &gRoots[threadIndex]);
return new CProfileIterator(&gRoots[threadIndex]);
}
void CProfileManager::CleanupMemory(void)
void CProfileManager::CleanupMemory(void)
{
for (int i=0;i<BT_QUICKPROF_MAX_THREAD_COUNT;i++)
for (int i = 0; i < BT_QUICKPROF_MAX_THREAD_COUNT; i++)
{
gRoots[i].CleanupMemory();
}
}
/***********************************************************************************************
* CProfileManager::Start_Profile -- Begin a named profile *
* *
@@ -546,51 +568,47 @@ void CProfileManager::CleanupMemory(void)
* The string used is assumed to be a static string; pointer compares are used throughout *
* the profiling code for efficiency. *
*=============================================================================================*/
void CProfileManager::Start_Profile( const char * name )
void CProfileManager::Start_Profile(const char* name)
{
int threadIndex = btQuickprofGetCurrentThreadIndex2();
if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
return;
if (name != gCurrentNodes[threadIndex]->Get_Name()) {
gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Sub_Node( name );
if (name != gCurrentNodes[threadIndex]->Get_Name())
{
gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Sub_Node(name);
}
gCurrentNodes[threadIndex]->Call();
}
/***********************************************************************************************
* CProfileManager::Stop_Profile -- Stop timing and record the results. *
*=============================================================================================*/
void CProfileManager::Stop_Profile( void )
void CProfileManager::Stop_Profile(void)
{
int threadIndex = btQuickprofGetCurrentThreadIndex2();
if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
return;
// Return will indicate whether we should back up to our parent (we may
// be profiling a recursive function)
if (gCurrentNodes[threadIndex]->Return()) {
if (gCurrentNodes[threadIndex]->Return())
{
gCurrentNodes[threadIndex] = gCurrentNodes[threadIndex]->Get_Parent();
}
}
/***********************************************************************************************
* CProfileManager::Reset -- Reset the contents of the profiling system *
* *
* This resets everything except for the tree structure. All of the timing data is reset. *
*=============================================================================================*/
void CProfileManager::Reset( void )
void CProfileManager::Reset(void)
{
gProfileClock.reset();
int threadIndex = btQuickprofGetCurrentThreadIndex2();
if ((threadIndex<0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
if ((threadIndex < 0) || threadIndex >= BT_QUICKPROF_MAX_THREAD_COUNT)
return;
gRoots[threadIndex].Reset();
gRoots[threadIndex].Call();
@@ -598,20 +616,18 @@ void CProfileManager::Reset( void )
Profile_Get_Ticks(&ResetTime);
}
/***********************************************************************************************
* CProfileManager::Increment_Frame_Counter -- Increment the frame counter *
*=============================================================================================*/
void CProfileManager::Increment_Frame_Counter( void )
void CProfileManager::Increment_Frame_Counter(void)
{
FrameCounter++;
}
/***********************************************************************************************
* CProfileManager::Get_Time_Since_Reset -- returns the elapsed time since last reset *
*=============================================================================================*/
float CProfileManager::Get_Time_Since_Reset( void )
float CProfileManager::Get_Time_Since_Reset(void)
{
unsigned long int time;
Profile_Get_Ticks(&time);
@@ -621,34 +637,34 @@ float CProfileManager::Get_Time_Since_Reset( void )
#include <stdio.h>
void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing)
void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spacing)
{
profileIterator->First();
if (profileIterator->Is_Done())
return;
float accumulated_time=0,parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time();
float accumulated_time = 0, parent_time = profileIterator->Is_Root() ? CProfileManager::Get_Time_Since_Reset() : profileIterator->Get_Current_Parent_Total_Time();
int i;
int frames_since_reset = CProfileManager::Get_Frame_Count_Since_Reset();
for (i=0;i<spacing;i++) printf(".");
for (i = 0; i < spacing; i++) printf(".");
printf("----------------------------------\n");
for (i=0;i<spacing;i++) printf(".");
printf("Profiling: %s (total running time: %.3f ms) ---\n", profileIterator->Get_Current_Parent_Name(), parent_time );
for (i = 0; i < spacing; i++) printf(".");
printf("Profiling: %s (total running time: %.3f ms) ---\n", profileIterator->Get_Current_Parent_Name(), parent_time);
float totalTime = 0.f;
int numChildren = 0;
for (i = 0; !profileIterator->Is_Done(); i++,profileIterator->Next())
for (i = 0; !profileIterator->Is_Done(); i++, profileIterator->Next())
{
numChildren++;
float current_total_time = profileIterator->Get_Current_Total_Time();
accumulated_time += current_total_time;
float fraction = parent_time > SIMD_EPSILON ? (current_total_time / parent_time) * 100 : 0.f;
{
int i; for (i=0;i<spacing;i++) printf(".");
int i;
for (i = 0; i < spacing; i++) printf(".");
}
printf("%d -- %s (%.2f %%) :: %.3f ms / frame (%d calls)\n",i, profileIterator->Get_Current_Name(), fraction,(current_total_time / (double)frames_since_reset),profileIterator->Get_Current_Total_Calls());
printf("%d -- %s (%.2f %%) :: %.3f ms / frame (%d calls)\n", i, profileIterator->Get_Current_Name(), fraction, (current_total_time / (double)frames_since_reset), profileIterator->Get_Current_Total_Calls());
totalTime += current_total_time;
//recurse into children
}
@@ -657,25 +673,23 @@ void CProfileManager::dumpRecursive(CProfileIterator* profileIterator, int spaci
{
//printf("what's wrong\n");
}
for (i=0;i<spacing;i++) printf(".");
printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:",parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time);
for (i = 0; i < spacing; i++) printf(".");
printf("%s (%.3f %%) :: %.3f ms\n", "Unaccounted:", parent_time > SIMD_EPSILON ? ((parent_time - accumulated_time) / parent_time) * 100 : 0.f, parent_time - accumulated_time);
for (i=0;i<numChildren;i++)
for (i = 0; i < numChildren; i++)
{
profileIterator->Enter_Child(i);
dumpRecursive(profileIterator,spacing+3);
dumpRecursive(profileIterator, spacing + 3);
profileIterator->Enter_Parent();
}
}
void CProfileManager::dumpAll()
void CProfileManager::dumpAll()
{
CProfileIterator* profileIterator = 0;
profileIterator = CProfileManager::Get_Iterator();
dumpRecursive(profileIterator,0);
dumpRecursive(profileIterator, 0);
CProfileManager::Release_Iterator(profileIterator);
}
@@ -703,50 +717,47 @@ void CProfileManager::dumpAll()
#endif // defined(__ANDROID__) && defined(__clang__)
// clang-format on
unsigned int btQuickprofGetCurrentThreadIndex2() {
const unsigned int kNullIndex = ~0U;
unsigned int btQuickprofGetCurrentThreadIndex2()
{
const unsigned int kNullIndex = ~0U;
#if BT_THREADSAFE
return btGetCurrentThreadIndex();
return btGetCurrentThreadIndex();
#else
#if defined(BT_HAVE_TLS)
static __thread unsigned int sThreadIndex = kNullIndex;
static __thread unsigned int sThreadIndex = kNullIndex;
#elif defined(_WIN32)
__declspec(thread) static unsigned int sThreadIndex = kNullIndex;
__declspec(thread) static unsigned int sThreadIndex = kNullIndex;
#else
unsigned int sThreadIndex = 0;
return -1;
unsigned int sThreadIndex = 0;
return -1;
#endif
static int gThreadCounter = 0;
static int gThreadCounter = 0;
if (sThreadIndex == kNullIndex) {
sThreadIndex = gThreadCounter++;
}
return sThreadIndex;
#endif //BT_THREADSAFE
if (sThreadIndex == kNullIndex)
{
sThreadIndex = gThreadCounter++;
}
return sThreadIndex;
#endif //BT_THREADSAFE
}
void btEnterProfileZoneDefault(const char* name)
void btEnterProfileZoneDefault(const char* name)
{
}
void btLeaveProfileZoneDefault()
void btLeaveProfileZoneDefault()
{
}
#else
void btEnterProfileZoneDefault(const char* name)
void btEnterProfileZoneDefault(const char* name)
{
}
void btLeaveProfileZoneDefault()
void btLeaveProfileZoneDefault()
{
}
#endif //BT_NO_PROFILE
#endif //BT_NO_PROFILE
static btEnterProfileZoneFunc* bts_enterFunc = btEnterProfileZoneDefault;
static btLeaveProfileZoneFunc* bts_leaveFunc = btLeaveProfileZoneDefault;
@@ -762,14 +773,13 @@ void btLeaveProfileZone()
btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc()
{
return bts_enterFunc ;
return bts_enterFunc;
}
btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc()
{
return bts_leaveFunc;
}
void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc)
{
bts_enterFunc = enterFunc;
@@ -779,13 +789,12 @@ void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc)
bts_leaveFunc = leaveFunc;
}
CProfileSample::CProfileSample( const char * name )
{
CProfileSample::CProfileSample(const char* name)
{
btEnterProfileZone(name);
}
CProfileSample::~CProfileSample( void )
{
CProfileSample::~CProfileSample(void)
{
btLeaveProfileZone();
}

199
src/LinearMath/btQuickprof.h
View File

@@ -7,11 +7,9 @@
**
***************************************************************************************************/
// Credits: The Clock class was inspired by the Timer classes in
// Credits: The Clock class was inspired by the Timer classes in
// Ogre (www.ogre3d.org).
#ifndef BT_QUICK_PROF_H
#define BT_QUICK_PROF_H
@@ -34,97 +32,87 @@ public:
/// Resets the initial reference time.
void reset();
/// Returns the time in ms since the last call to reset or since
/// Returns the time in ms since the last call to reset or since
/// the btClock was created.
unsigned long long int getTimeMilliseconds();
/// Returns the time in us since the last call to reset or since
/// Returns the time in us since the last call to reset or since
/// the Clock was created.
unsigned long long int getTimeMicroseconds();
unsigned long long int getTimeNanoseconds();
/// Returns the time in s since the last call to reset or since
/// Returns the time in s since the last call to reset or since
/// the Clock was created.
btScalar getTimeSeconds();
private:
struct btClockData* m_data;
};
#endif //USE_BT_CLOCK
#endif //USE_BT_CLOCK
typedef void (btEnterProfileZoneFunc)(const char* msg);
typedef void (btLeaveProfileZoneFunc)();
typedef void(btEnterProfileZoneFunc)(const char* msg);
typedef void(btLeaveProfileZoneFunc)();
btEnterProfileZoneFunc* btGetCurrentEnterProfileZoneFunc();
btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc();
void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc);
void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc);
#ifndef BT_NO_PROFILE // FIX redefinition
#ifndef BT_NO_PROFILE // FIX redefinition
//To disable built-in profiling, please comment out next line
//#define BT_NO_PROFILE 1
#endif //BT_NO_PROFILE
#endif //BT_NO_PROFILE
const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64;
#ifndef BT_NO_PROFILE
//btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined,
//btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined,
//otherwise returns thread index in range [0..maxThreads]
unsigned int btQuickprofGetCurrentThreadIndex2();
#include <stdio.h>//@todo remove this, backwards compatibility
#include <stdio.h> //@todo remove this, backwards compatibility
#include "btAlignedAllocator.h"
#include <new>
///A node in the Profile Hierarchy Tree
class CProfileNode {
class CProfileNode
{
public:
CProfileNode( const char * name, CProfileNode * parent );
~CProfileNode( void );
CProfileNode(const char* name, CProfileNode* parent);
~CProfileNode(void);
CProfileNode * Get_Sub_Node( const char * name );
CProfileNode* Get_Sub_Node(const char* name);
CProfileNode * Get_Parent( void ) { return Parent; }
CProfileNode * Get_Sibling( void ) { return Sibling; }
CProfileNode * Get_Child( void ) { return Child; }
CProfileNode* Get_Parent(void) { return Parent; }
CProfileNode* Get_Sibling(void) { return Sibling; }
CProfileNode* Get_Child(void) { return Child; }
void CleanupMemory();
void Reset( void );
void Call( void );
bool Return( void );
void CleanupMemory();
void Reset(void);
void Call(void);
bool Return(void);
const char* Get_Name(void) { return Name; }
int Get_Total_Calls(void) { return TotalCalls; }
float Get_Total_Time(void) { return TotalTime; }
void* GetUserPointer() const { return m_userPtr; }
void SetUserPointer(void* ptr) { m_userPtr = ptr; }
const char * Get_Name( void ) { return Name; }
int Get_Total_Calls( void ) { return TotalCalls; }
float Get_Total_Time( void ) { return TotalTime; }
void* GetUserPointer() const {return m_userPtr;}
void SetUserPointer(void* ptr) { m_userPtr = ptr;}
protected:
const char* Name;
int TotalCalls;
float TotalTime;
unsigned long int StartTime;
int RecursionCounter;
const char * Name;
int TotalCalls;
float TotalTime;
unsigned long int StartTime;
int RecursionCounter;
CProfileNode * Parent;
CProfileNode * Child;
CProfileNode * Sibling;
void* m_userPtr;
CProfileNode* Parent;
CProfileNode* Child;
CProfileNode* Sibling;
void* m_userPtr;
};
///An iterator to navigate through the tree
@@ -132,91 +120,80 @@ class CProfileIterator
{
public:
// Access all the children of the current parent
void First(void);
void Next(void);
bool Is_Done(void);
bool Is_Root(void) { return (CurrentParent->Get_Parent() == 0); }
void First(void);
void Next(void);
bool Is_Done(void);
bool Is_Root(void) { return (CurrentParent->Get_Parent() == 0); }
void Enter_Child( int index ); // Make the given child the new parent
void Enter_Largest_Child( void ); // Make the largest child the new parent
void Enter_Parent( void ); // Make the current parent's parent the new parent
void Enter_Child(int index); // Make the given child the new parent
void Enter_Largest_Child(void); // Make the largest child the new parent
void Enter_Parent(void); // Make the current parent's parent the new parent
// Access the current child
const char * Get_Current_Name( void ) { return CurrentChild->Get_Name(); }
int Get_Current_Total_Calls( void ) { return CurrentChild->Get_Total_Calls(); }
float Get_Current_Total_Time( void ) { return CurrentChild->Get_Total_Time(); }
const char* Get_Current_Name(void) { return CurrentChild->Get_Name(); }
int Get_Current_Total_Calls(void) { return CurrentChild->Get_Total_Calls(); }
float Get_Current_Total_Time(void) { return CurrentChild->Get_Total_Time(); }
void* Get_Current_UserPointer( void ) { return CurrentChild->GetUserPointer(); }
void Set_Current_UserPointer(void* ptr) {CurrentChild->SetUserPointer(ptr);}
void* Get_Current_UserPointer(void) { return CurrentChild->GetUserPointer(); }
void Set_Current_UserPointer(void* ptr) { CurrentChild->SetUserPointer(ptr); }
// Access the current parent
const char * Get_Current_Parent_Name( void ) { return CurrentParent->Get_Name(); }
int Get_Current_Parent_Total_Calls( void ) { return CurrentParent->Get_Total_Calls(); }
float Get_Current_Parent_Total_Time( void ) { return CurrentParent->Get_Total_Time(); }
const char* Get_Current_Parent_Name(void) { return CurrentParent->Get_Name(); }
int Get_Current_Parent_Total_Calls(void) { return CurrentParent->Get_Total_Calls(); }
float Get_Current_Parent_Total_Time(void) { return CurrentParent->Get_Total_Time(); }
protected:
CProfileNode* CurrentParent;
CProfileNode* CurrentChild;
CProfileNode * CurrentParent;
CProfileNode * CurrentChild;
CProfileIterator( CProfileNode * start );
friend class CProfileManager;
CProfileIterator(CProfileNode* start);
friend class CProfileManager;
};
///The Manager for the Profile system
class CProfileManager {
class CProfileManager
{
public:
static void Start_Profile( const char * name );
static void Stop_Profile( void );
static void Start_Profile(const char* name);
static void Stop_Profile(void);
static void CleanupMemory(void);
// {
// Root.CleanupMemory();
// }
static void CleanupMemory(void);
// {
// Root.CleanupMemory();
// }
static void Reset( void );
static void Increment_Frame_Counter( void );
static int Get_Frame_Count_Since_Reset( void ) { return FrameCounter; }
static float Get_Time_Since_Reset( void );
static void Reset(void);
static void Increment_Frame_Counter(void);
static int Get_Frame_Count_Since_Reset(void) { return FrameCounter; }
static float Get_Time_Since_Reset(void);
static CProfileIterator * Get_Iterator( void );
// {
//
// return new CProfileIterator( &Root );
// }
static void Release_Iterator( CProfileIterator * iterator ) { delete ( iterator); }
static CProfileIterator* Get_Iterator(void);
// {
//
// return new CProfileIterator( &Root );
// }
static void Release_Iterator(CProfileIterator* iterator) { delete (iterator); }
static void dumpRecursive(CProfileIterator* profileIterator, int spacing);
static void dumpRecursive(CProfileIterator* profileIterator, int spacing);
static void dumpAll();
static void dumpAll();
private:
static int FrameCounter;
static unsigned long int ResetTime;
static int FrameCounter;
static unsigned long int ResetTime;
};
#endif //#ifndef BT_NO_PROFILE
#endif //#ifndef BT_NO_PROFILE
///ProfileSampleClass is a simple way to profile a function's scope
///Use the BT_PROFILE macro at the start of scope to time
class CProfileSample {
class CProfileSample
{
public:
CProfileSample( const char * name );
CProfileSample(const char* name);
~CProfileSample( void );
~CProfileSample(void);
};
#define BT_PROFILE( name ) CProfileSample __profile( name )
#endif //BT_QUICK_PROF_H
#define BT_PROFILE(name) CProfileSample __profile(name)
#endif //BT_QUICK_PROF_H

13
src/LinearMath/btRandom.h
View File

@@ -12,8 +12,6 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_GEN_RANDOM_H
#define BT_GEN_RANDOM_H
@@ -24,8 +22,8 @@ subject to the following restrictions:
#define GEN_RAND_MAX UINT_MAX
SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { init_genrand(seed); }
SIMD_FORCE_INLINE unsigned int GEN_rand() { return genrand_int32(); }
SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { init_genrand(seed); }
SIMD_FORCE_INLINE unsigned int GEN_rand() { return genrand_int32(); }
#else
@@ -33,10 +31,9 @@ SIMD_FORCE_INLINE unsigned int GEN_rand() { return genrand_int
#define GEN_RAND_MAX RAND_MAX
SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { srand(seed); }
SIMD_FORCE_INLINE unsigned int GEN_rand() { return rand(); }
SIMD_FORCE_INLINE void GEN_srand(unsigned int seed) { srand(seed); }
SIMD_FORCE_INLINE unsigned int GEN_rand() { return rand(); }
#endif
#endif //BT_GEN_RANDOM_H
#endif //BT_GEN_RANDOM_H

18
src/LinearMath/btScalar.h
View File

@@ -32,7 +32,6 @@ inline int btGetVersion()
return BT_BULLET_VERSION;
}
// The following macro "BT_NOT_EMPTY_FILE" can be put into a file
// in order suppress the MS Visual C++ Linker warning 4221
//
@@ -44,16 +43,19 @@ inline int btGetVersion()
//
// see more https://stackoverflow.com/questions/1822887/what-is-the-best-way-to-eliminate-ms-visual-c-linker-warning-warning-lnk422
#if defined (_MSC_VER)
#define BT_NOT_EMPTY_FILE_CAT_II(p, res) res
#define BT_NOT_EMPTY_FILE_CAT_I(a, b) BT_NOT_EMPTY_FILE_CAT_II(~, a ## b)
#define BT_NOT_EMPTY_FILE_CAT(a, b) BT_NOT_EMPTY_FILE_CAT_I(a, b)
#define BT_NOT_EMPTY_FILE namespace { char BT_NOT_EMPTY_FILE_CAT(NoEmptyFileDummy, __COUNTER__); }
#if defined(_MSC_VER)
#define BT_NOT_EMPTY_FILE_CAT_II(p, res) res
#define BT_NOT_EMPTY_FILE_CAT_I(a, b) BT_NOT_EMPTY_FILE_CAT_II(~, a##b)
#define BT_NOT_EMPTY_FILE_CAT(a, b) BT_NOT_EMPTY_FILE_CAT_I(a, b)
#define BT_NOT_EMPTY_FILE \
namespace \
{ \
char BT_NOT_EMPTY_FILE_CAT(NoEmptyFileDummy, __COUNTER__); \
}
#else
#define BT_NOT_EMPTY_FILE
#define BT_NOT_EMPTY_FILE
#endif
// clang and most formatting tools don't support indentation of preprocessor guards, so turn it off
// clang-format off
#if defined(DEBUG) || defined (_DEBUG)

3
src/LinearMath/btSerializer.cpp
View File

@@ -1,3 +1,4 @@
// clang-format off
char sBulletDNAstr[]= {
char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-76),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109),
char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95),
@@ -687,3 +688,5 @@ char(97),char(0),char(4),char(0),char(50),char(0),char(-79),char(1),char(96),cha
char(98),char(0),char(4),char(0),char(48),char(0),char(-79),char(1),char(95),char(0),char(-78),char(1),char(4),char(0),char(-77),char(1),char(0),char(0),char(37),char(0),
};
int sBulletDNAlen= sizeof(sBulletDNAstr);
// clang-format on

1265
src/LinearMath/btSerializer.h
View File

File diff suppressed because it is too large Load Diff

2
src/LinearMath/btSerializer64.cpp
View File

@@ -1,3 +1,4 @@
// clang-format off
char sBulletDNAstr64[]= {
char(83),char(68),char(78),char(65),char(78),char(65),char(77),char(69),char(-76),char(1),char(0),char(0),char(109),char(95),char(115),char(105),char(122),char(101),char(0),char(109),
char(95),char(99),char(97),char(112),char(97),char(99),char(105),char(116),char(121),char(0),char(42),char(109),char(95),char(100),char(97),char(116),char(97),char(0),char(109),char(95),
@@ -687,3 +688,4 @@ char(97),char(0),char(4),char(0),char(50),char(0),char(-79),char(1),char(96),cha
char(98),char(0),char(4),char(0),char(48),char(0),char(-79),char(1),char(95),char(0),char(-78),char(1),char(4),char(0),char(-77),char(1),char(0),char(0),char(37),char(0),
};
int sBulletDNAlen64= sizeof(sBulletDNAstr64);
// clang-format on

248
src/LinearMath/btSpatialAlgebra.h
View File

@@ -12,18 +12,17 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
///These spatial algebra classes are used for btMultiBody,
///These spatial algebra classes are used for btMultiBody,
///see BulletDynamics/Featherstone
#ifndef BT_SPATIAL_ALGEBRA_H
#define BT_SPATIAL_ALGEBRA_H
#include "btMatrix3x3.h"
struct btSpatialForceVector
{
btVector3 m_topVec, m_bottomVec;
{
btVector3 m_topVec, m_bottomVec;
//
btSpatialForceVector() { setZero(); }
btSpatialForceVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(linear), m_bottomVec(angular) {}
@@ -32,21 +31,34 @@ struct btSpatialForceVector
setValue(ax, ay, az, lx, ly, lz);
}
//
void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = linear; m_bottomVec = angular; }
void setVector(const btVector3 &angular, const btVector3 &linear)
{
m_topVec = linear;
m_bottomVec = angular;
}
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
{
m_bottomVec.setValue(ax, ay, az); m_topVec.setValue(lx, ly, lz);
m_bottomVec.setValue(ax, ay, az);
m_topVec.setValue(lx, ly, lz);
}
//
void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
void addVector(const btVector3 &angular, const btVector3 &linear)
{
m_topVec += linear;
m_bottomVec += angular;
}
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
{
m_bottomVec[0] += ax; m_bottomVec[1] += ay; m_bottomVec[2] += az;
m_topVec[0] += lx; m_topVec[1] += ly; m_topVec[2] += lz;
m_bottomVec[0] += ax;
m_bottomVec[1] += ay;
m_bottomVec[2] += az;
m_topVec[0] += lx;
m_topVec[1] += ly;
m_topVec[2] += lz;
}
//
const btVector3 & getLinear() const { return m_topVec; }
const btVector3 & getAngular() const { return m_bottomVec; }
const btVector3 &getLinear() const { return m_topVec; }
const btVector3 &getAngular() const { return m_bottomVec; }
//
void setLinear(const btVector3 &linear) { m_topVec = linear; }
void setAngular(const btVector3 &angular) { m_bottomVec = angular; }
@@ -54,14 +66,28 @@ struct btSpatialForceVector
void addAngular(const btVector3 &angular) { m_bottomVec += angular; }
void addLinear(const btVector3 &linear) { m_topVec += linear; }
//
void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
void setZero()
{
m_topVec.setZero();
m_bottomVec.setZero();
}
//
btSpatialForceVector & operator += (const btSpatialForceVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
btSpatialForceVector & operator -= (const btSpatialForceVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
btSpatialForceVector operator - (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); }
btSpatialForceVector operator + (const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); }
btSpatialForceVector operator - () const { return btSpatialForceVector(-m_bottomVec, -m_topVec); }
btSpatialForceVector operator * (const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); }
btSpatialForceVector &operator+=(const btSpatialForceVector &vec)
{
m_topVec += vec.m_topVec;
m_bottomVec += vec.m_bottomVec;
return *this;
}
btSpatialForceVector &operator-=(const btSpatialForceVector &vec)
{
m_topVec -= vec.m_topVec;
m_bottomVec -= vec.m_bottomVec;
return *this;
}
btSpatialForceVector operator-(const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec - vec.m_bottomVec, m_topVec - vec.m_topVec); }
btSpatialForceVector operator+(const btSpatialForceVector &vec) const { return btSpatialForceVector(m_bottomVec + vec.m_bottomVec, m_topVec + vec.m_topVec); }
btSpatialForceVector operator-() const { return btSpatialForceVector(-m_bottomVec, -m_topVec); }
btSpatialForceVector operator*(const btScalar &s) const { return btSpatialForceVector(s * m_bottomVec, s * m_topVec); }
//btSpatialForceVector & operator = (const btSpatialForceVector &vec) { m_topVec = vec.m_topVec; m_bottomVec = vec.m_bottomVec; return *this; }
};
@@ -70,23 +96,36 @@ struct btSpatialMotionVector
btVector3 m_topVec, m_bottomVec;
//
btSpatialMotionVector() { setZero(); }
btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}
btSpatialMotionVector(const btVector3 &angular, const btVector3 &linear) : m_topVec(angular), m_bottomVec(linear) {}
//
void setVector(const btVector3 &angular, const btVector3 &linear) { m_topVec = angular; m_bottomVec = linear; }
void setVector(const btVector3 &angular, const btVector3 &linear)
{
m_topVec = angular;
m_bottomVec = linear;
}
void setValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
{
m_topVec.setValue(ax, ay, az); m_bottomVec.setValue(lx, ly, lz);
m_topVec.setValue(ax, ay, az);
m_bottomVec.setValue(lx, ly, lz);
}
//
void addVector(const btVector3 &angular, const btVector3 &linear) { m_topVec += linear; m_bottomVec += angular; }
void addVector(const btVector3 &angular, const btVector3 &linear)
{
m_topVec += linear;
m_bottomVec += angular;
}
void addValue(const btScalar &ax, const btScalar &ay, const btScalar &az, const btScalar &lx, const btScalar &ly, const btScalar &lz)
{
m_topVec[0] += ax; m_topVec[1] += ay; m_topVec[2] += az;
m_bottomVec[0] += lx; m_bottomVec[1] += ly; m_bottomVec[2] += lz;
m_topVec[0] += ax;
m_topVec[1] += ay;
m_topVec[2] += az;
m_bottomVec[0] += lx;
m_bottomVec[1] += ly;
m_bottomVec[2] += lz;
}
//
const btVector3 & getAngular() const { return m_topVec; }
const btVector3 & getLinear() const { return m_bottomVec; }
//
const btVector3 &getAngular() const { return m_topVec; }
const btVector3 &getLinear() const { return m_bottomVec; }
//
void setAngular(const btVector3 &angular) { m_topVec = angular; }
void setLinear(const btVector3 &linear) { m_bottomVec = linear; }
@@ -94,20 +133,24 @@ struct btSpatialMotionVector
void addAngular(const btVector3 &angular) { m_topVec += angular; }
void addLinear(const btVector3 &linear) { m_bottomVec += linear; }
//
void setZero() { m_topVec.setZero(); m_bottomVec.setZero(); }
void setZero()
{
m_topVec.setZero();
m_bottomVec.setZero();
}
//
btScalar dot(const btSpatialForceVector &b) const
{
return m_bottomVec.dot(b.m_topVec) + m_topVec.dot(b.m_bottomVec);
}
//
template<typename SpatialVectorType>
template <typename SpatialVectorType>
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
{
out.m_topVec = m_topVec.cross(b.m_topVec);
out.m_bottomVec = m_bottomVec.cross(b.m_topVec) + m_topVec.cross(b.m_bottomVec);
}
template<typename SpatialVectorType>
template <typename SpatialVectorType>
SpatialVectorType cross(const SpatialVectorType &b) const
{
SpatialVectorType out;
@@ -116,21 +159,36 @@ struct btSpatialMotionVector
return out;
}
//
btSpatialMotionVector & operator += (const btSpatialMotionVector &vec) { m_topVec += vec.m_topVec; m_bottomVec += vec.m_bottomVec; return *this; }
btSpatialMotionVector & operator -= (const btSpatialMotionVector &vec) { m_topVec -= vec.m_topVec; m_bottomVec -= vec.m_bottomVec; return *this; }
btSpatialMotionVector & operator *= (const btScalar &s) { m_topVec *= s; m_bottomVec *= s; return *this; }
btSpatialMotionVector operator - (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
btSpatialMotionVector operator + (const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
btSpatialMotionVector operator - () const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
btSpatialMotionVector operator * (const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); }
btSpatialMotionVector &operator+=(const btSpatialMotionVector &vec)
{
m_topVec += vec.m_topVec;
m_bottomVec += vec.m_bottomVec;
return *this;
}
btSpatialMotionVector &operator-=(const btSpatialMotionVector &vec)
{
m_topVec -= vec.m_topVec;
m_bottomVec -= vec.m_bottomVec;
return *this;
}
btSpatialMotionVector &operator*=(const btScalar &s)
{
m_topVec *= s;
m_bottomVec *= s;
return *this;
}
btSpatialMotionVector operator-(const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec - vec.m_topVec, m_bottomVec - vec.m_bottomVec); }
btSpatialMotionVector operator+(const btSpatialMotionVector &vec) const { return btSpatialMotionVector(m_topVec + vec.m_topVec, m_bottomVec + vec.m_bottomVec); }
btSpatialMotionVector operator-() const { return btSpatialMotionVector(-m_topVec, -m_bottomVec); }
btSpatialMotionVector operator*(const btScalar &s) const { return btSpatialMotionVector(s * m_topVec, s * m_bottomVec); }
};
struct btSymmetricSpatialDyad
{
btMatrix3x3 m_topLeftMat, m_topRightMat, m_bottomLeftMat;
//
//
btSymmetricSpatialDyad() { setIdentity(); }
btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }
btSymmetricSpatialDyad(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat) { setMatrix(topLeftMat, topRightMat, bottomLeftMat); }
//
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
{
@@ -146,17 +204,22 @@ struct btSymmetricSpatialDyad
m_bottomLeftMat += bottomLeftMat;
}
//
void setIdentity() { m_topLeftMat.setIdentity(); m_topRightMat.setIdentity(); m_bottomLeftMat.setIdentity(); }
void setIdentity()
{
m_topLeftMat.setIdentity();
m_topRightMat.setIdentity();
m_bottomLeftMat.setIdentity();
}
//
btSymmetricSpatialDyad & operator -= (const btSymmetricSpatialDyad &mat)
btSymmetricSpatialDyad &operator-=(const btSymmetricSpatialDyad &mat)
{
m_topLeftMat -= mat.m_topLeftMat;
m_topRightMat -= mat.m_topRightMat;
m_bottomLeftMat -= mat.m_bottomLeftMat;
return *this;
return *this;
}
//
btSpatialForceVector operator * (const btSpatialMotionVector &vec)
btSpatialForceVector operator*(const btSpatialMotionVector &vec)
{
return btSpatialForceVector(m_bottomLeftMat * vec.m_topVec + m_topLeftMat.transpose() * vec.m_bottomVec, m_topLeftMat * vec.m_topVec + m_topRightMat * vec.m_bottomVec);
}
@@ -164,7 +227,7 @@ struct btSymmetricSpatialDyad
struct btSpatialTransformationMatrix
{
btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
btMatrix3x3 m_rotMat; //btMatrix3x3 m_trnCrossMat;
btVector3 m_trnVec;
//
enum eOutputOperation
@@ -174,128 +237,124 @@ struct btSpatialTransformationMatrix
Subtract = 2
};
//
template<typename SpatialVectorType>
void transform( const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
template <typename SpatialVectorType>
void transform(const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
{
if(outOp == None)
if (outOp == None)
{
outVec.m_topVec = m_rotMat * inVec.m_topVec;
outVec.m_bottomVec = -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
}
else if(outOp == Add)
else if (outOp == Add)
{
outVec.m_topVec += m_rotMat * inVec.m_topVec;
outVec.m_bottomVec += -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
}
else if(outOp == Subtract)
else if (outOp == Subtract)
{
outVec.m_topVec -= m_rotMat * inVec.m_topVec;
outVec.m_bottomVec -= -m_trnVec.cross(outVec.m_topVec) + m_rotMat * inVec.m_bottomVec;
}
}
template<typename SpatialVectorType>
void transformRotationOnly( const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
template <typename SpatialVectorType>
void transformRotationOnly(const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
{
if(outOp == None)
if (outOp == None)
{
outVec.m_topVec = m_rotMat * inVec.m_topVec;
outVec.m_bottomVec = m_rotMat * inVec.m_bottomVec;
}
else if(outOp == Add)
else if (outOp == Add)
{
outVec.m_topVec += m_rotMat * inVec.m_topVec;
outVec.m_bottomVec += m_rotMat * inVec.m_bottomVec;
}
else if(outOp == Subtract)
else if (outOp == Subtract)
{
outVec.m_topVec -= m_rotMat * inVec.m_topVec;
outVec.m_bottomVec -= m_rotMat * inVec.m_bottomVec;
}
}
template<typename SpatialVectorType>
void transformInverse( const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
template <typename SpatialVectorType>
void transformInverse(const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
{
if(outOp == None)
if (outOp == None)
{
outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec = m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
}
else if(outOp == Add)
else if (outOp == Add)
{
outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec += m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
}
else if(outOp == Subtract)
else if (outOp == Subtract)
{
outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec -= m_rotMat.transpose() * (inVec.m_bottomVec + m_trnVec.cross(inVec.m_topVec));
}
}
}
template<typename SpatialVectorType>
void transformInverseRotationOnly( const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
template <typename SpatialVectorType>
void transformInverseRotationOnly(const SpatialVectorType &inVec,
SpatialVectorType &outVec,
eOutputOperation outOp = None)
{
if(outOp == None)
if (outOp == None)
{
outVec.m_topVec = m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec = m_rotMat.transpose() * inVec.m_bottomVec;
}
else if(outOp == Add)
else if (outOp == Add)
{
outVec.m_topVec += m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec += m_rotMat.transpose() * inVec.m_bottomVec;
}
else if(outOp == Subtract)
else if (outOp == Subtract)
{
outVec.m_topVec -= m_rotMat.transpose() * inVec.m_topVec;
outVec.m_bottomVec -= m_rotMat.transpose() * inVec.m_bottomVec;
}
}
void transformInverse( const btSymmetricSpatialDyad &inMat,
btSymmetricSpatialDyad &outMat,
eOutputOperation outOp = None)
void transformInverse(const btSymmetricSpatialDyad &inMat,
btSymmetricSpatialDyad &outMat,
eOutputOperation outOp = None)
{
const btMatrix3x3 r_cross( 0, -m_trnVec[2], m_trnVec[1],
m_trnVec[2], 0, -m_trnVec[0],
-m_trnVec[1], m_trnVec[0], 0);
const btMatrix3x3 r_cross(0, -m_trnVec[2], m_trnVec[1],
m_trnVec[2], 0, -m_trnVec[0],
-m_trnVec[1], m_trnVec[0], 0);
if(outOp == None)
if (outOp == None)
{
outMat.m_topLeftMat = m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
outMat.m_topLeftMat = m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat;
outMat.m_topRightMat = m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
outMat.m_bottomLeftMat = m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
}
else if(outOp == Add)
else if (outOp == Add)
{
outMat.m_topLeftMat += m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
outMat.m_topLeftMat += m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat;
outMat.m_topRightMat += m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
outMat.m_bottomLeftMat += m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
}
else if(outOp == Subtract)
else if (outOp == Subtract)
{
outMat.m_topLeftMat -= m_rotMat.transpose() * ( inMat.m_topLeftMat - inMat.m_topRightMat * r_cross ) * m_rotMat;
outMat.m_topLeftMat -= m_rotMat.transpose() * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) * m_rotMat;
outMat.m_topRightMat -= m_rotMat.transpose() * inMat.m_topRightMat * m_rotMat;
outMat.m_bottomLeftMat -= m_rotMat.transpose() * (r_cross * (inMat.m_topLeftMat - inMat.m_topRightMat * r_cross) + inMat.m_bottomLeftMat - inMat.m_topLeftMat.transpose() * r_cross) * m_rotMat;
}
}
template<typename SpatialVectorType>
SpatialVectorType operator * (const SpatialVectorType &vec)
template <typename SpatialVectorType>
SpatialVectorType operator*(const SpatialVectorType &vec)
{
SpatialVectorType out;
transform(vec, out);
@@ -303,7 +362,7 @@ struct btSpatialTransformationMatrix
}
};
template<typename SpatialVectorType>
template <typename SpatialVectorType>
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
{
//output op maybe?
@@ -314,7 +373,7 @@ void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVecto
//maybe simple a*spatTranspose(a) would be nicer?
}
template<typename SpatialVectorType>
template <typename SpatialVectorType>
btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b)
{
btSymmetricSpatialDyad out;
@@ -327,5 +386,4 @@ btSymmetricSpatialDyad symmetricSpatialOuterProduct(const SpatialVectorType &a,
//maybe simple a*spatTranspose(a) would be nicer?
}
#endif //BT_SPATIAL_ALGEBRA_H
#endif //BT_SPATIAL_ALGEBRA_H

96
src/LinearMath/btStackAlloc.h
View File

@@ -20,97 +20,99 @@ Nov.2006
#ifndef BT_STACK_ALLOC
#define BT_STACK_ALLOC
#include "btScalar.h" //for btAssert
#include "btScalar.h" //for btAssert
#include "btAlignedAllocator.h"
///The btBlock class is an internal structure for the btStackAlloc memory allocator.
struct btBlock
{
btBlock* previous;
unsigned char* address;
btBlock* previous;
unsigned char* address;
};
///The StackAlloc class provides some fast stack-based memory allocator (LIFO last-in first-out)
class btStackAlloc
{
public:
btStackAlloc(unsigned int size)
{
ctor();
create(size);
}
~btStackAlloc() { destroy(); }
btStackAlloc(unsigned int size) { ctor();create(size); }
~btStackAlloc() { destroy(); }
inline void create(unsigned int size)
inline void create(unsigned int size)
{
destroy();
data = (unsigned char*) btAlignedAlloc(size,16);
totalsize = size;
data = (unsigned char*)btAlignedAlloc(size, 16);
totalsize = size;
}
inline void destroy()
inline void destroy()
{
btAssert(usedsize==0);
btAssert(usedsize == 0);
//Raise(L"StackAlloc is still in use");
if(usedsize==0)
if (usedsize == 0)
{
if(!ischild && data)
if (!ischild && data)
btAlignedFree(data);
data = 0;
usedsize = 0;
data = 0;
usedsize = 0;
}
}
int getAvailableMemory() const
int getAvailableMemory() const
{
return static_cast<int>(totalsize - usedsize);
}
unsigned char* allocate(unsigned int size)
unsigned char* allocate(unsigned int size)
{
const unsigned int nus(usedsize+size);
if(nus<totalsize)
const unsigned int nus(usedsize + size);
if (nus < totalsize)
{
usedsize=nus;
return(data+(usedsize-size));
usedsize = nus;
return (data + (usedsize - size));
}
btAssert(0);
//&& (L"Not enough memory"));
return(0);
return (0);
}
SIMD_FORCE_INLINE btBlock* beginBlock()
SIMD_FORCE_INLINE btBlock* beginBlock()
{
btBlock* pb = (btBlock*)allocate(sizeof(btBlock));
pb->previous = current;
pb->address = data+usedsize;
current = pb;
return(pb);
btBlock* pb = (btBlock*)allocate(sizeof(btBlock));
pb->previous = current;
pb->address = data + usedsize;
current = pb;
return (pb);
}
SIMD_FORCE_INLINE void endBlock(btBlock* block)
SIMD_FORCE_INLINE void endBlock(btBlock* block)
{
btAssert(block==current);
btAssert(block == current);
//Raise(L"Unmatched blocks");
if(block==current)
if (block == current)
{
current = block->previous;
usedsize = (unsigned int)((block->address-data)-sizeof(btBlock));
current = block->previous;
usedsize = (unsigned int)((block->address - data) - sizeof(btBlock));
}
}
private:
void ctor()
void ctor()
{
data = 0;
totalsize = 0;
usedsize = 0;
current = 0;
ischild = false;
data = 0;
totalsize = 0;
usedsize = 0;
current = 0;
ischild = false;
}
unsigned char* data;
unsigned int totalsize;
unsigned int usedsize;
btBlock* current;
bool ischild;
unsigned char* data;
unsigned int totalsize;
unsigned int usedsize;
btBlock* current;
bool ischild;
};
#endif //BT_STACK_ALLOC
#endif //BT_STACK_ALLOC

927
src/LinearMath/btThreads.cpp
View File

File diff suppressed because it is too large Load Diff

97
src/LinearMath/btThreads.h
View File

@@ -12,14 +12,12 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_THREADS_H
#define BT_THREADS_H
#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
#include "btScalar.h" // has definitions like SIMD_FORCE_INLINE
#if defined (_MSC_VER) && _MSC_VER >= 1600
#if defined(_MSC_VER) && _MSC_VER >= 1600
// give us a compile error if any signatures of overriden methods is changed
#define BT_OVERRIDE override
#endif
@@ -36,7 +34,7 @@ const unsigned int BT_MAX_THREAD_COUNT = 64; // only if BT_THREADSAFE is 1
bool btIsMainThread();
bool btThreadsAreRunning();
unsigned int btGetCurrentThreadIndex();
void btResetThreadIndexCounter(); // notify that all worker threads have been destroyed
void btResetThreadIndexCounter(); // notify that all worker threads have been destroyed
///
/// btSpinMutex -- lightweight spin-mutex implemented with atomic ops, never puts
@@ -46,19 +44,18 @@ void btResetThreadIndexCounter(); // notify that all worker threads have been de
///
class btSpinMutex
{
int mLock;
int mLock;
public:
btSpinMutex()
{
mLock = 0;
}
void lock();
void unlock();
bool tryLock();
btSpinMutex()
{
mLock = 0;
}
void lock();
void unlock();
bool tryLock();
};
//
// NOTE: btMutex* is for internal Bullet use only
//
@@ -70,43 +67,42 @@ public:
// of bad because if you call any of these functions from external code
// (where BT_THREADSAFE is undefined) you will get unexpected race conditions.
//
SIMD_FORCE_INLINE void btMutexLock( btSpinMutex* mutex )
SIMD_FORCE_INLINE void btMutexLock(btSpinMutex* mutex)
{
#if BT_THREADSAFE
mutex->lock();
mutex->lock();
#else
(void)mutex;
#endif // #if BT_THREADSAFE
(void)mutex;
#endif // #if BT_THREADSAFE
}
SIMD_FORCE_INLINE void btMutexUnlock( btSpinMutex* mutex )
SIMD_FORCE_INLINE void btMutexUnlock(btSpinMutex* mutex)
{
#if BT_THREADSAFE
mutex->unlock();
mutex->unlock();
#else
(void)mutex;
#endif // #if BT_THREADSAFE
(void)mutex;
#endif // #if BT_THREADSAFE
}
SIMD_FORCE_INLINE bool btMutexTryLock( btSpinMutex* mutex )
SIMD_FORCE_INLINE bool btMutexTryLock(btSpinMutex* mutex)
{
#if BT_THREADSAFE
return mutex->tryLock();
return mutex->tryLock();
#else
(void)mutex;
return true;
#endif // #if BT_THREADSAFE
(void)mutex;
return true;
#endif // #if BT_THREADSAFE
}
//
// btIParallelForBody -- subclass this to express work that can be done in parallel
//
class btIParallelForBody
{
public:
virtual ~btIParallelForBody() {}
virtual void forLoop( int iBegin, int iEnd ) const = 0;
virtual ~btIParallelForBody() {}
virtual void forLoop(int iBegin, int iEnd) const = 0;
};
//
@@ -116,8 +112,8 @@ public:
class btIParallelSumBody
{
public:
virtual ~btIParallelSumBody() {}
virtual btScalar sumLoop( int iBegin, int iEnd ) const = 0;
virtual ~btIParallelSumBody() {}
virtual btScalar sumLoop(int iBegin, int iEnd) const = 0;
};
//
@@ -127,30 +123,30 @@ public:
class btITaskScheduler
{
public:
btITaskScheduler( const char* name );
virtual ~btITaskScheduler() {}
const char* getName() const { return m_name; }
btITaskScheduler(const char* name);
virtual ~btITaskScheduler() {}
const char* getName() const { return m_name; }
virtual int getMaxNumThreads() const = 0;
virtual int getNumThreads() const = 0;
virtual void setNumThreads( int numThreads ) = 0;
virtual void parallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body ) = 0;
virtual btScalar parallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body ) = 0;
virtual void sleepWorkerThreadsHint() {} // hint the task scheduler that we may not be using these threads for a little while
virtual int getMaxNumThreads() const = 0;
virtual int getNumThreads() const = 0;
virtual void setNumThreads(int numThreads) = 0;
virtual void parallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody& body) = 0;
virtual btScalar parallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body) = 0;
virtual void sleepWorkerThreadsHint() {} // hint the task scheduler that we may not be using these threads for a little while
// internal use only
virtual void activate();
virtual void deactivate();
// internal use only
virtual void activate();
virtual void deactivate();
protected:
const char* m_name;
unsigned int m_savedThreadCounter;
bool m_isActive;
const char* m_name;
unsigned int m_savedThreadCounter;
bool m_isActive;
};
// set the task scheduler to use for all calls to btParallelFor()
// NOTE: you must set this prior to using any of the multi-threaded "Mt" classes
void btSetTaskScheduler( btITaskScheduler* ts );
void btSetTaskScheduler(btITaskScheduler* ts);
// get the current task scheduler
btITaskScheduler* btGetTaskScheduler();
@@ -172,11 +168,10 @@ btITaskScheduler* btGetPPLTaskScheduler();
// btParallelFor -- call this to dispatch work like a for-loop
// (iterations may be done out of order, so no dependencies are allowed)
void btParallelFor( int iBegin, int iEnd, int grainSize, const btIParallelForBody& body );
void btParallelFor(int iBegin, int iEnd, int grainSize, const btIParallelForBody& body);
// btParallelSum -- call this to dispatch work like a for-loop, returns the sum of all iterations
// (iterations may be done out of order, so no dependencies are allowed)
btScalar btParallelSum( int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body );
btScalar btParallelSum(int iBegin, int iEnd, int grainSize, const btIParallelSumBody& body);
#endif

217
src/LinearMath/btTransform.h
View File

@@ -12,12 +12,9 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_TRANSFORM_H
#define BT_TRANSFORM_H
#include "btMatrix3x3.h"
#ifdef BT_USE_DOUBLE_PRECISION
@@ -26,46 +23,45 @@ subject to the following restrictions:
#define btTransformData btTransformFloatData
#endif
/**@brief The btTransform class supports rigid transforms with only translation and rotation and no scaling/shear.
*It can be used in combination with btVector3, btQuaternion and btMatrix3x3 linear algebra classes. */
ATTRIBUTE_ALIGNED16(class) btTransform {
///Storage for the rotation
ATTRIBUTE_ALIGNED16(class)
btTransform
{
///Storage for the rotation
btMatrix3x3 m_basis;
///Storage for the translation
btVector3 m_origin;
///Storage for the translation
btVector3 m_origin;
public:
/**@brief No initialization constructor */
/**@brief No initialization constructor */
btTransform() {}
/**@brief Constructor from btQuaternion (optional btVector3 )
/**@brief Constructor from btQuaternion (optional btVector3 )
* @param q Rotation from quaternion
* @param c Translation from Vector (default 0,0,0) */
explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q,
const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
explicit SIMD_FORCE_INLINE btTransform(const btQuaternion& q,
const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
: m_basis(q),
m_origin(c)
{}
/**@brief Constructor from btMatrix3x3 (optional btVector3)
* @param b Rotation from Matrix
* @param c Translation from Vector default (0,0,0)*/
explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b,
const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
: m_basis(b),
m_origin(c)
{}
/**@brief Copy constructor */
SIMD_FORCE_INLINE btTransform (const btTransform& other)
: m_basis(other.m_basis),
m_origin(other.m_origin)
m_origin(c)
{
}
/**@brief Assignment Operator */
/**@brief Constructor from btMatrix3x3 (optional btVector3)
* @param b Rotation from Matrix
* @param c Translation from Vector default (0,0,0)*/
explicit SIMD_FORCE_INLINE btTransform(const btMatrix3x3& b,
const btVector3& c = btVector3(btScalar(0), btScalar(0), btScalar(0)))
: m_basis(b),
m_origin(c)
{
}
/**@brief Copy constructor */
SIMD_FORCE_INLINE btTransform(const btTransform& other)
: m_basis(other.m_basis),
m_origin(other.m_origin)
{
}
/**@brief Assignment Operator */
SIMD_FORCE_INLINE btTransform& operator=(const btTransform& other)
{
m_basis = other.m_basis;
@@ -73,70 +69,70 @@ public:
return *this;
}
/**@brief Set the current transform as the value of the product of two transforms
/**@brief Set the current transform as the value of the product of two transforms
* @param t1 Transform 1
* @param t2 Transform 2
* This = Transform1 * Transform2 */
SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2) {
m_basis = t1.m_basis * t2.m_basis;
m_origin = t1(t2.m_origin);
}
SIMD_FORCE_INLINE void mult(const btTransform& t1, const btTransform& t2)
{
m_basis = t1.m_basis * t2.m_basis;
m_origin = t1(t2.m_origin);
}
/* void multInverseLeft(const btTransform& t1, const btTransform& t2) {
/* void multInverseLeft(const btTransform& t1, const btTransform& t2) {
btVector3 v = t2.m_origin - t1.m_origin;
m_basis = btMultTransposeLeft(t1.m_basis, t2.m_basis);
m_origin = v * t1.m_basis;
}
*/
/**@brief Return the transform of the vector */
/**@brief Return the transform of the vector */
SIMD_FORCE_INLINE btVector3 operator()(const btVector3& x) const
{
return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
return x.dot3(m_basis[0], m_basis[1], m_basis[2]) + m_origin;
}
/**@brief Return the transform of the vector */
/**@brief Return the transform of the vector */
SIMD_FORCE_INLINE btVector3 operator*(const btVector3& x) const
{
return (*this)(x);
}
/**@brief Return the transform of the btQuaternion */
/**@brief Return the transform of the btQuaternion */
SIMD_FORCE_INLINE btQuaternion operator*(const btQuaternion& q) const
{
return getRotation() * q;
}
/**@brief Return the basis matrix for the rotation */
SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; }
/**@brief Return the basis matrix for the rotation */
SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; }
/**@brief Return the basis matrix for the rotation */
SIMD_FORCE_INLINE btMatrix3x3& getBasis() { return m_basis; }
/**@brief Return the basis matrix for the rotation */
SIMD_FORCE_INLINE const btMatrix3x3& getBasis() const { return m_basis; }
/**@brief Return the origin vector translation */
SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; }
/**@brief Return the origin vector translation */
SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; }
/**@brief Return the origin vector translation */
SIMD_FORCE_INLINE btVector3& getOrigin() { return m_origin; }
/**@brief Return the origin vector translation */
SIMD_FORCE_INLINE const btVector3& getOrigin() const { return m_origin; }
/**@brief Return a quaternion representing the rotation */
btQuaternion getRotation() const {
/**@brief Return a quaternion representing the rotation */
btQuaternion getRotation() const
{
btQuaternion q;
m_basis.getRotation(q);
return q;
}
/**@brief Set from an array
/**@brief Set from an array
* @param m A pointer to a 16 element array (12 rotation(row major padded on the right by 1), and 3 translation */
void setFromOpenGLMatrix(const btScalar *m)
void setFromOpenGLMatrix(const btScalar* m)
{
m_basis.setFromOpenGLSubMatrix(m);
m_origin.setValue(m[12],m[13],m[14]);
m_origin.setValue(m[12], m[13], m[14]);
}
/**@brief Fill an array representation
/**@brief Fill an array representation
* @param m A pointer to a 16 element array (12 rotation(row major padded on the right by 1), and 3 translation */
void getOpenGLMatrix(btScalar *m) const
void getOpenGLMatrix(btScalar * m) const
{
m_basis.getOpenGLSubMatrix(m);
m[12] = m_origin.x();
@@ -145,80 +141,76 @@ public:
m[15] = btScalar(1.0);
}
/**@brief Set the translational element
/**@brief Set the translational element
* @param origin The vector to set the translation to */
SIMD_FORCE_INLINE void setOrigin(const btVector3& origin)
{
SIMD_FORCE_INLINE void setOrigin(const btVector3& origin)
{
m_origin = origin;
}
SIMD_FORCE_INLINE btVector3 invXform(const btVector3& inVec) const;
/**@brief Set the rotational element by btMatrix3x3 */
/**@brief Set the rotational element by btMatrix3x3 */
SIMD_FORCE_INLINE void setBasis(const btMatrix3x3& basis)
{
{
m_basis = basis;
}
/**@brief Set the rotational element by btQuaternion */
/**@brief Set the rotational element by btQuaternion */
SIMD_FORCE_INLINE void setRotation(const btQuaternion& q)
{
m_basis.setRotation(q);
}
/**@brief Set this transformation to the identity */
/**@brief Set this transformation to the identity */
void setIdentity()
{
m_basis.setIdentity();
m_origin.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
}
/**@brief Multiply this Transform by another(this = this * another)
/**@brief Multiply this Transform by another(this = this * another)
* @param t The other transform */
btTransform& operator*=(const btTransform& t)
btTransform& operator*=(const btTransform& t)
{
m_origin += m_basis * t.m_origin;
m_basis *= t.m_basis;
return *this;
}
/**@brief Return the inverse of this transform */
/**@brief Return the inverse of this transform */
btTransform inverse() const
{
{
btMatrix3x3 inv = m_basis.transpose();
return btTransform(inv, inv * -m_origin);
}
/**@brief Return the inverse of this transform times the other transform
/**@brief Return the inverse of this transform times the other transform
* @param t The other transform
* return this.inverse() * the other */
btTransform inverseTimes(const btTransform& t) const;
btTransform inverseTimes(const btTransform& t) const;
/**@brief Return the product of this transform and the other */
/**@brief Return the product of this transform and the other */
btTransform operator*(const btTransform& t) const;
/**@brief Return an identity transform */
static const btTransform& getIdentity()
/**@brief Return an identity transform */
static const btTransform& getIdentity()
{
static const btTransform identityTransform(btMatrix3x3::getIdentity());
return identityTransform;
}
void serialize(struct btTransformData& dataOut) const;
void serialize(struct btTransformData & dataOut) const;
void serializeFloat(struct btTransformFloatData& dataOut) const;
void serializeFloat(struct btTransformFloatData & dataOut) const;
void deSerialize(const struct btTransformData& dataIn);
void deSerialize(const struct btTransformData& dataIn);
void deSerializeDouble(const struct btTransformDoubleData& dataIn);
void deSerializeFloat(const struct btTransformFloatData& dataIn);
void deSerializeDouble(const struct btTransformDoubleData& dataIn);
void deSerializeFloat(const struct btTransformFloatData& dataIn);
};
SIMD_FORCE_INLINE btVector3
btTransform::invXform(const btVector3& inVec) const
{
@@ -226,80 +218,69 @@ btTransform::invXform(const btVector3& inVec) const
return (m_basis.transpose() * v);
}
SIMD_FORCE_INLINE btTransform
btTransform::inverseTimes(const btTransform& t) const
SIMD_FORCE_INLINE btTransform
btTransform::inverseTimes(const btTransform& t) const
{
btVector3 v = t.getOrigin() - m_origin;
return btTransform(m_basis.transposeTimes(t.m_basis),
v * m_basis);
return btTransform(m_basis.transposeTimes(t.m_basis),
v * m_basis);
}
SIMD_FORCE_INLINE btTransform
btTransform::operator*(const btTransform& t) const
SIMD_FORCE_INLINE btTransform
btTransform::operator*(const btTransform& t) const
{
return btTransform(m_basis * t.m_basis,
(*this)(t.m_origin));
return btTransform(m_basis * t.m_basis,
(*this)(t.m_origin));
}
/**@brief Test if two transforms have all elements equal */
SIMD_FORCE_INLINE bool operator==(const btTransform& t1, const btTransform& t2)
{
return ( t1.getBasis() == t2.getBasis() &&
t1.getOrigin() == t2.getOrigin() );
return (t1.getBasis() == t2.getBasis() &&
t1.getOrigin() == t2.getOrigin());
}
///for serialization
struct btTransformFloatData
struct btTransformFloatData
{
btMatrix3x3FloatData m_basis;
btVector3FloatData m_origin;
btMatrix3x3FloatData m_basis;
btVector3FloatData m_origin;
};
struct btTransformDoubleData
struct btTransformDoubleData
{
btMatrix3x3DoubleData m_basis;
btVector3DoubleData m_origin;
btMatrix3x3DoubleData m_basis;
btVector3DoubleData m_origin;
};
SIMD_FORCE_INLINE void btTransform::serialize(btTransformData& dataOut) const
SIMD_FORCE_INLINE void btTransform::serialize(btTransformData& dataOut) const
{
m_basis.serialize(dataOut.m_basis);
m_origin.serialize(dataOut.m_origin);
}
SIMD_FORCE_INLINE void btTransform::serializeFloat(btTransformFloatData& dataOut) const
SIMD_FORCE_INLINE void btTransform::serializeFloat(btTransformFloatData& dataOut) const
{
m_basis.serializeFloat(dataOut.m_basis);
m_origin.serializeFloat(dataOut.m_origin);
}
SIMD_FORCE_INLINE void btTransform::deSerialize(const btTransformData& dataIn)
SIMD_FORCE_INLINE void btTransform::deSerialize(const btTransformData& dataIn)
{
m_basis.deSerialize(dataIn.m_basis);
m_origin.deSerialize(dataIn.m_origin);
}
SIMD_FORCE_INLINE void btTransform::deSerializeFloat(const btTransformFloatData& dataIn)
SIMD_FORCE_INLINE void btTransform::deSerializeFloat(const btTransformFloatData& dataIn)
{
m_basis.deSerializeFloat(dataIn.m_basis);
m_origin.deSerializeFloat(dataIn.m_origin);
}
SIMD_FORCE_INLINE void btTransform::deSerializeDouble(const btTransformDoubleData& dataIn)
SIMD_FORCE_INLINE void btTransform::deSerializeDouble(const btTransformDoubleData& dataIn)
{
m_basis.deSerializeDouble(dataIn.m_basis);
m_origin.deSerializeDouble(dataIn.m_origin);
}
#endif //BT_TRANSFORM_H
#endif //BT_TRANSFORM_H

158
src/LinearMath/btTransformUtil.h
View File

@@ -12,77 +12,66 @@ subject to the following restrictions:
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_TRANSFORM_UTIL_H
#define BT_TRANSFORM_UTIL_H
#include "btTransform.h"
#define ANGULAR_MOTION_THRESHOLD btScalar(0.5)*SIMD_HALF_PI
#define ANGULAR_MOTION_THRESHOLD btScalar(0.5) * SIMD_HALF_PI
SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents,const btVector3& supportDir)
SIMD_FORCE_INLINE btVector3 btAabbSupport(const btVector3& halfExtents, const btVector3& supportDir)
{
return btVector3(supportDir.x() < btScalar(0.0) ? -halfExtents.x() : halfExtents.x(),
supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z());
supportDir.y() < btScalar(0.0) ? -halfExtents.y() : halfExtents.y(),
supportDir.z() < btScalar(0.0) ? -halfExtents.z() : halfExtents.z());
}
/// Utils related to temporal transforms
class btTransformUtil
{
public:
static void integrateTransform(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep,btTransform& predictedTransform)
static void integrateTransform(const btTransform& curTrans, const btVector3& linvel, const btVector3& angvel, btScalar timeStep, btTransform& predictedTransform)
{
predictedTransform.setOrigin(curTrans.getOrigin() + linvel * timeStep);
// #define QUATERNION_DERIVATIVE
#ifdef QUATERNION_DERIVATIVE
// #define QUATERNION_DERIVATIVE
#ifdef QUATERNION_DERIVATIVE
btQuaternion predictedOrn = curTrans.getRotation();
predictedOrn += (angvel * predictedOrn) * (timeStep * btScalar(0.5));
predictedOrn.safeNormalize();
#else
#else
//Exponential map
//google for "Practical Parameterization of Rotations Using the Exponential Map", F. Sebastian Grassia
btVector3 axis;
btScalar fAngle2 = angvel.length2();
btScalar fAngle = 0;
if (fAngle2>SIMD_EPSILON)
{
fAngle = btSqrt(fAngle2);
}
btScalar fAngle2 = angvel.length2();
btScalar fAngle = 0;
if (fAngle2 > SIMD_EPSILON)
{
fAngle = btSqrt(fAngle2);
}
//limit the angular motion
if (fAngle*timeStep > ANGULAR_MOTION_THRESHOLD)
if (fAngle * timeStep > ANGULAR_MOTION_THRESHOLD)
{
fAngle = ANGULAR_MOTION_THRESHOLD / timeStep;
}
if ( fAngle < btScalar(0.001) )
if (fAngle < btScalar(0.001))
{
// use Taylor's expansions of sync function
axis = angvel*( btScalar(0.5)*timeStep-(timeStep*timeStep*timeStep)*(btScalar(0.020833333333))*fAngle*fAngle );
axis = angvel * (btScalar(0.5) * timeStep - (timeStep * timeStep * timeStep) * (btScalar(0.020833333333)) * fAngle * fAngle);
}
else
{
// sync(fAngle) = sin(c*fAngle)/t
axis = angvel*( btSin(btScalar(0.5)*fAngle*timeStep)/fAngle );
axis = angvel * (btSin(btScalar(0.5) * fAngle * timeStep) / fAngle);
}
btQuaternion dorn (axis.x(),axis.y(),axis.z(),btCos( fAngle*timeStep*btScalar(0.5) ));
btQuaternion dorn(axis.x(), axis.y(), axis.z(), btCos(fAngle * timeStep * btScalar(0.5)));
btQuaternion orn0 = curTrans.getRotation();
btQuaternion predictedOrn = dorn * orn0;
predictedOrn.safeNormalize();
#endif
if (predictedOrn.length2()>SIMD_EPSILON)
#endif
if (predictedOrn.length2() > SIMD_EPSILON)
{
predictedTransform.setRotation(predictedOrn);
}
@@ -92,137 +81,133 @@ public:
}
}
static void calculateVelocityQuaternion(const btVector3& pos0,const btVector3& pos1,const btQuaternion& orn0,const btQuaternion& orn1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
static void calculateVelocityQuaternion(const btVector3& pos0, const btVector3& pos1, const btQuaternion& orn0, const btQuaternion& orn1, btScalar timeStep, btVector3& linVel, btVector3& angVel)
{
linVel = (pos1 - pos0) / timeStep;
btVector3 axis;
btScalar angle;
btScalar angle;
if (orn0 != orn1)
{
calculateDiffAxisAngleQuaternion(orn0,orn1,axis,angle);
calculateDiffAxisAngleQuaternion(orn0, orn1, axis, angle);
angVel = axis * angle / timeStep;
} else
}
else
{
angVel.setValue(0,0,0);
angVel.setValue(0, 0, 0);
}
}
static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0,const btQuaternion& orn1a,btVector3& axis,btScalar& angle)
static void calculateDiffAxisAngleQuaternion(const btQuaternion& orn0, const btQuaternion& orn1a, btVector3& axis, btScalar& angle)
{
btQuaternion orn1 = orn0.nearest(orn1a);
btQuaternion dorn = orn1 * orn0.inverse();
angle = dorn.getAngle();
axis = btVector3(dorn.x(),dorn.y(),dorn.z());
axis = btVector3(dorn.x(), dorn.y(), dorn.z());
axis[3] = btScalar(0.);
//check for axis length
btScalar len = axis.length2();
if (len < SIMD_EPSILON*SIMD_EPSILON)
axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
if (len < SIMD_EPSILON * SIMD_EPSILON)
axis = btVector3(btScalar(1.), btScalar(0.), btScalar(0.));
else
axis /= btSqrt(len);
}
static void calculateVelocity(const btTransform& transform0,const btTransform& transform1,btScalar timeStep,btVector3& linVel,btVector3& angVel)
static void calculateVelocity(const btTransform& transform0, const btTransform& transform1, btScalar timeStep, btVector3& linVel, btVector3& angVel)
{
linVel = (transform1.getOrigin() - transform0.getOrigin()) / timeStep;
btVector3 axis;
btScalar angle;
calculateDiffAxisAngle(transform0,transform1,axis,angle);
btScalar angle;
calculateDiffAxisAngle(transform0, transform1, axis, angle);
angVel = axis * angle / timeStep;
}
static void calculateDiffAxisAngle(const btTransform& transform0,const btTransform& transform1,btVector3& axis,btScalar& angle)
static void calculateDiffAxisAngle(const btTransform& transform0, const btTransform& transform1, btVector3& axis, btScalar& angle)
{
btMatrix3x3 dmat = transform1.getBasis() * transform0.getBasis().inverse();
btQuaternion dorn;
dmat.getRotation(dorn);
///floating point inaccuracy can lead to w component > 1..., which breaks
///floating point inaccuracy can lead to w component > 1..., which breaks
dorn.normalize();
angle = dorn.getAngle();
axis = btVector3(dorn.x(),dorn.y(),dorn.z());
axis = btVector3(dorn.x(), dorn.y(), dorn.z());
axis[3] = btScalar(0.);
//check for axis length
btScalar len = axis.length2();
if (len < SIMD_EPSILON*SIMD_EPSILON)
axis = btVector3(btScalar(1.),btScalar(0.),btScalar(0.));
if (len < SIMD_EPSILON * SIMD_EPSILON)
axis = btVector3(btScalar(1.), btScalar(0.), btScalar(0.));
else
axis /= btSqrt(len);
}
};
///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
///The btConvexSeparatingDistanceUtil can help speed up convex collision detection
///by conservatively updating a cached separating distance/vector instead of re-calculating the closest distance
class btConvexSeparatingDistanceUtil
class btConvexSeparatingDistanceUtil
{
btQuaternion m_ornA;
btQuaternion m_ornB;
btVector3 m_posA;
btVector3 m_posB;
btVector3 m_separatingNormal;
btQuaternion m_ornA;
btQuaternion m_ornB;
btVector3 m_posA;
btVector3 m_posB;
btScalar m_boundingRadiusA;
btScalar m_boundingRadiusB;
btScalar m_separatingDistance;
btVector3 m_separatingNormal;
btScalar m_boundingRadiusA;
btScalar m_boundingRadiusB;
btScalar m_separatingDistance;
public:
btConvexSeparatingDistanceUtil(btScalar boundingRadiusA,btScalar boundingRadiusB)
:m_boundingRadiusA(boundingRadiusA),
m_boundingRadiusB(boundingRadiusB),
m_separatingDistance(0.f)
btConvexSeparatingDistanceUtil(btScalar boundingRadiusA, btScalar boundingRadiusB)
: m_boundingRadiusA(boundingRadiusA),
m_boundingRadiusB(boundingRadiusB),
m_separatingDistance(0.f)
{
}
btScalar getConservativeSeparatingDistance()
btScalar getConservativeSeparatingDistance()
{
return m_separatingDistance;
}
void updateSeparatingDistance(const btTransform& transA,const btTransform& transB)
void updateSeparatingDistance(const btTransform& transA, const btTransform& transB)
{
const btVector3& toPosA = transA.getOrigin();
const btVector3& toPosB = transB.getOrigin();
btQuaternion toOrnA = transA.getRotation();
btQuaternion toOrnB = transB.getRotation();
if (m_separatingDistance>0.f)
if (m_separatingDistance > 0.f)
{
btVector3 linVelA,angVelA,linVelB,angVelB;
btTransformUtil::calculateVelocityQuaternion(m_posA,toPosA,m_ornA,toOrnA,btScalar(1.),linVelA,angVelA);
btTransformUtil::calculateVelocityQuaternion(m_posB,toPosB,m_ornB,toOrnB,btScalar(1.),linVelB,angVelB);
btVector3 linVelA, angVelA, linVelB, angVelB;
btTransformUtil::calculateVelocityQuaternion(m_posA, toPosA, m_ornA, toOrnA, btScalar(1.), linVelA, angVelA);
btTransformUtil::calculateVelocityQuaternion(m_posB, toPosB, m_ornB, toOrnB, btScalar(1.), linVelB, angVelB);
btScalar maxAngularProjectedVelocity = angVelA.length() * m_boundingRadiusA + angVelB.length() * m_boundingRadiusB;
btVector3 relLinVel = (linVelB-linVelA);
btVector3 relLinVel = (linVelB - linVelA);
btScalar relLinVelocLength = relLinVel.dot(m_separatingNormal);
if (relLinVelocLength<0.f)
if (relLinVelocLength < 0.f)
{
relLinVelocLength = 0.f;
}
btScalar projectedMotion = maxAngularProjectedVelocity +relLinVelocLength;
btScalar projectedMotion = maxAngularProjectedVelocity + relLinVelocLength;
m_separatingDistance -= projectedMotion;
}
m_posA = toPosA;
m_posB = toPosB;
m_ornA = toOrnA;
m_ornB = toOrnB;
}
void initSeparatingDistance(const btVector3& separatingVector,btScalar separatingDistance,const btTransform& transA,const btTransform& transB)
void initSeparatingDistance(const btVector3& separatingVector, btScalar separatingDistance, const btTransform& transA, const btTransform& transB)
{
m_separatingDistance = separatingDistance;
if (m_separatingDistance>0.f)
if (m_separatingDistance > 0.f)
{
m_separatingNormal = separatingVector;
const btVector3& toPosA = transA.getOrigin();
const btVector3& toPosB = transB.getOrigin();
btQuaternion toOrnA = transA.getRotation();
@@ -233,9 +218,6 @@ public:
m_ornB = toOrnB;
}
}
};
#endif //BT_TRANSFORM_UTIL_H
#endif //BT_TRANSFORM_UTIL_H

3014
src/LinearMath/btVector3.cpp
View File

File diff suppressed because it is too large Load Diff

1061
src/LinearMath/btVector3.h
View File

File diff suppressed because it is too large Load Diff