Several changes to sync Bullet trunk with PlayStation 3 spubullet version
Still needs some cross-platform fixes
This commit is contained in:
erwin.coumans
334
src/BulletMultiThreaded/TrbStateVec.h
Normal file
334
src/BulletMultiThreaded/TrbStateVec.h
Normal file
@@ -0,0 +1,334 @@
|
||||
/*
|
||||
Copyright (C) 2009 Sony Computer Entertainment Inc.
|
||||
All rights reserved.
|
||||
|
||||
This software is provided 'as-is', without any express or implied warranty.
|
||||
In no event will the authors be held liable for any damages arising from the use of this software.
|
||||
Permission is granted to anyone to use this software for any purpose,
|
||||
including commercial applications, and to alter it and redistribute it freely,
|
||||
subject to the following restrictions:
|
||||
|
||||
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
|
||||
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
|
||||
*/
|
||||
|
||||
#ifndef __TRBSTATEVEC_H__
|
||||
#define __TRBSTATEVEC_H__
|
||||
|
||||
#include <stdlib.h>
|
||||
#include <vectormath_aos.h>
|
||||
|
||||
|
||||
#include "PlatformDefinitions.h"
|
||||
|
||||
|
||||
static inline vmVector3 read_Vector3(const float* p)
|
||||
{
|
||||
vmVector3 v;
|
||||
loadXYZ(v, p);
|
||||
return v;
|
||||
}
|
||||
|
||||
static inline vmQuat read_Quat(const float* p)
|
||||
{
|
||||
vmQuat vq;
|
||||
loadXYZW(vq, p);
|
||||
return vq;
|
||||
}
|
||||
|
||||
static inline void store_Vector3(const vmVector3 &src, float* p)
|
||||
{
|
||||
vmVector3 v = src;
|
||||
storeXYZ(v, p);
|
||||
}
|
||||
|
||||
static inline void store_Quat(const vmQuat &src, float* p)
|
||||
{
|
||||
vmQuat vq = src;
|
||||
storeXYZW(vq, p);
|
||||
}
|
||||
|
||||
// Motion Type
|
||||
enum {
|
||||
PfxMotionTypeFixed = 0,
|
||||
PfxMotionTypeActive,
|
||||
PfxMotionTypeKeyframe,
|
||||
PfxMotionTypeOneWay,
|
||||
PfxMotionTypeTrigger,
|
||||
PfxMotionTypeCount
|
||||
};
|
||||
|
||||
#define PFX_MOTION_MASK_DYNAMIC 0x0a // Active,OneWay
|
||||
#define PFX_MOTION_MASK_STATIC 0x95 // Fixed,Keyframe,Trigger,Sleeping
|
||||
#define PFX_MOTION_MASK_SLEEP 0x0e // Can sleep
|
||||
#define PFX_MOTION_MASK_TYPE 0x7f
|
||||
|
||||
//
|
||||
// Rigid Body state
|
||||
//
|
||||
|
||||
#ifdef __CELLOS_LV2__
|
||||
ATTRIBUTE_ALIGNED128(class) TrbState
|
||||
#else
|
||||
ATTRIBUTE_ALIGNED16(class) TrbState
|
||||
#endif
|
||||
|
||||
{
|
||||
public:
|
||||
TrbState()
|
||||
{
|
||||
setMotionType(PfxMotionTypeActive);
|
||||
contactFilterSelf=contactFilterTarget=0xffffffff;
|
||||
deleted = 0;
|
||||
mSleeping = 0;
|
||||
useSleep = 1;
|
||||
trbBodyIdx=0;
|
||||
mSleepCount=0;
|
||||
useCcd = 0;
|
||||
useContactCallback = 0;
|
||||
useSleepCallback = 0;
|
||||
linearDamping = 1.0f;
|
||||
angularDamping = 0.99f;
|
||||
}
|
||||
|
||||
TrbState(const uint8_t m, const vmVector3& x, const vmQuat& q, const vmVector3& v, const vmVector3& omega );
|
||||
|
||||
uint16_t mSleepCount;
|
||||
uint8_t mMotionType;
|
||||
uint8_t deleted : 1;
|
||||
uint8_t mSleeping : 1;
|
||||
uint8_t useSleep : 1;
|
||||
uint8_t useCcd : 1;
|
||||
uint8_t useContactCallback : 1;
|
||||
uint8_t useSleepCallback : 1;
|
||||
|
||||
uint16_t trbBodyIdx;
|
||||
uint32_t contactFilterSelf;
|
||||
uint32_t contactFilterTarget;
|
||||
|
||||
float center[3]; // AABB center(World)
|
||||
float half[3]; // AABB half(World)
|
||||
|
||||
float linearDamping;
|
||||
float angularDamping;
|
||||
|
||||
float deltaLinearVelocity[3];
|
||||
float deltaAngularVelocity[3];
|
||||
|
||||
float fX[3]; // position
|
||||
float fQ[4]; // orientation
|
||||
float fV[3]; // velocity
|
||||
float fOmega[3]; // angular velocity
|
||||
|
||||
inline void setZero(); // Zeroes out the elements
|
||||
inline void setIdentity(); // Sets the rotation to identity and zeroes out the other elements
|
||||
|
||||
bool isDeleted() const {return deleted==1;}
|
||||
|
||||
uint16_t getRigidBodyId() const {return trbBodyIdx;}
|
||||
void setRigidBodyId(uint16_t i) {trbBodyIdx = i;}
|
||||
|
||||
|
||||
uint32_t getContactFilterSelf() const {return contactFilterSelf;}
|
||||
void setContactFilterSelf(uint32_t filter) {contactFilterSelf = filter;}
|
||||
|
||||
uint32_t getContactFilterTarget() const {return contactFilterTarget;}
|
||||
void setContactFilterTarget(uint32_t filter) {contactFilterTarget = filter;}
|
||||
|
||||
float getLinearDamping() const {return linearDamping;}
|
||||
float getAngularDamping() const {return angularDamping;}
|
||||
|
||||
void setLinearDamping(float damping) {linearDamping=damping;}
|
||||
void setAngularDamping(float damping) {angularDamping=damping;}
|
||||
|
||||
|
||||
uint8_t getMotionType() const {return mMotionType;}
|
||||
void setMotionType(uint8_t t) {mMotionType = t;mSleeping=0;mSleepCount=0;}
|
||||
|
||||
uint8_t getMotionMask() const {return (1<<mMotionType)|(mSleeping<<7);}
|
||||
|
||||
bool isAsleep() const {return mSleeping==1;}
|
||||
bool isAwake() const {return mSleeping==0;}
|
||||
|
||||
void wakeup() {mSleeping=0;mSleepCount=0;}
|
||||
void sleep() {if(useSleep) {mSleeping=1;mSleepCount=0;}}
|
||||
|
||||
uint8_t getUseSleep() const {return useSleep;}
|
||||
void setUseSleep(uint8_t b) {useSleep=b;}
|
||||
|
||||
uint8_t getUseCcd() const {return useCcd;}
|
||||
void setUseCcd(uint8_t b) {useCcd=b;}
|
||||
|
||||
uint8_t getUseContactCallback() const {return useContactCallback;}
|
||||
void setUseContactCallback(uint8_t b) {useContactCallback=b;}
|
||||
|
||||
uint8_t getUseSleepCallback() const {return useSleepCallback;}
|
||||
void setUseSleepCallback(uint8_t b) {useSleepCallback=b;}
|
||||
|
||||
void incrementSleepCount() {mSleepCount++;}
|
||||
void resetSleepCount() {mSleepCount=0;}
|
||||
uint16_t getSleepCount() const {return mSleepCount;}
|
||||
|
||||
vmVector3 getPosition() const {return read_Vector3(fX);}
|
||||
vmQuat getOrientation() const {return read_Quat(fQ);}
|
||||
vmVector3 getLinearVelocity() const {return read_Vector3(fV);}
|
||||
vmVector3 getAngularVelocity() const {return read_Vector3(fOmega);}
|
||||
vmVector3 getDeltaLinearVelocity() const {return read_Vector3(deltaLinearVelocity);}
|
||||
vmVector3 getDeltaAngularVelocity() const {return read_Vector3(deltaAngularVelocity);}
|
||||
|
||||
void setPosition(const vmVector3 &pos) {store_Vector3(pos, fX);}
|
||||
void setLinearVelocity(const vmVector3 &vel) {store_Vector3(vel, fV);}
|
||||
void setAngularVelocity(const vmVector3 &vel) {store_Vector3(vel, fOmega);}
|
||||
void setDeltaLinearVelocity(const vmVector3 &vel) {store_Vector3(vel, deltaLinearVelocity);}
|
||||
void setDeltaAngularVelocity(const vmVector3 &vel) {store_Vector3(vel, deltaAngularVelocity);}
|
||||
void setOrientation(const vmQuat &rot) {store_Quat(rot, fQ);}
|
||||
|
||||
inline void setAuxils(const vmVector3 ¢erLocal,const vmVector3 &halfLocal);
|
||||
inline void setAuxilsCcd(const vmVector3 ¢erLocal,const vmVector3 &halfLocal,float timeStep);
|
||||
inline void reset();
|
||||
};
|
||||
|
||||
inline
|
||||
TrbState::TrbState(const uint8_t m, const vmVector3& x, const vmQuat& q, const vmVector3& v, const vmVector3& omega)
|
||||
{
|
||||
setMotionType(m);
|
||||
fX[0] = x[0];
|
||||
fX[1] = x[1];
|
||||
fX[2] = x[2];
|
||||
fQ[0] = q[0];
|
||||
fQ[1] = q[1];
|
||||
fQ[2] = q[2];
|
||||
fQ[3] = q[3];
|
||||
fV[0] = v[0];
|
||||
fV[1] = v[1];
|
||||
fV[2] = v[2];
|
||||
fOmega[0] = omega[0];
|
||||
fOmega[1] = omega[1];
|
||||
fOmega[2] = omega[2];
|
||||
contactFilterSelf=contactFilterTarget=0xffff;
|
||||
trbBodyIdx=0;
|
||||
mSleeping = 0;
|
||||
deleted = 0;
|
||||
useSleep = 1;
|
||||
useCcd = 0;
|
||||
useContactCallback = 0;
|
||||
useSleepCallback = 0;
|
||||
mSleepCount=0;
|
||||
linearDamping = 1.0f;
|
||||
angularDamping = 0.99f;
|
||||
}
|
||||
|
||||
inline void
|
||||
TrbState::setIdentity()
|
||||
{
|
||||
fX[0] = 0.0f;
|
||||
fX[1] = 0.0f;
|
||||
fX[2] = 0.0f;
|
||||
fQ[0] = 0.0f;
|
||||
fQ[1] = 0.0f;
|
||||
fQ[2] = 0.0f;
|
||||
fQ[3] = 1.0f;
|
||||
fV[0] = 0.0f;
|
||||
fV[1] = 0.0f;
|
||||
fV[2] = 0.0f;
|
||||
fOmega[0] = 0.0f;
|
||||
fOmega[1] = 0.0f;
|
||||
fOmega[2] = 0.0f;
|
||||
}
|
||||
|
||||
inline void
|
||||
TrbState::setZero()
|
||||
{
|
||||
fX[0] = 0.0f;
|
||||
fX[1] = 0.0f;
|
||||
fX[2] = 0.0f;
|
||||
fQ[0] = 0.0f;
|
||||
fQ[1] = 0.0f;
|
||||
fQ[2] = 0.0f;
|
||||
fQ[3] = 0.0f;
|
||||
fV[0] = 0.0f;
|
||||
fV[1] = 0.0f;
|
||||
fV[2] = 0.0f;
|
||||
fOmega[0] = 0.0f;
|
||||
fOmega[1] = 0.0f;
|
||||
fOmega[2] = 0.0f;
|
||||
}
|
||||
|
||||
inline void
|
||||
TrbState::setAuxils(const vmVector3 ¢erLocal,const vmVector3 &halfLocal)
|
||||
{
|
||||
vmVector3 centerW = getPosition() + rotate(getOrientation(),centerLocal);
|
||||
vmVector3 halfW = absPerElem(vmMatrix3(getOrientation())) * halfLocal;
|
||||
center[0] = centerW[0];
|
||||
center[1] = centerW[1];
|
||||
center[2] = centerW[2];
|
||||
half[0] = halfW[0];
|
||||
half[1] = halfW[1];
|
||||
half[2] = halfW[2];
|
||||
}
|
||||
|
||||
inline void
|
||||
TrbState::setAuxilsCcd(const vmVector3 ¢erLocal,const vmVector3 &halfLocal,float timeStep)
|
||||
{
|
||||
vmVector3 centerW = getPosition() + rotate(getOrientation(),centerLocal);
|
||||
vmVector3 halfW = absPerElem(vmMatrix3(getOrientation())) * halfLocal;
|
||||
|
||||
vmVector3 diffvec = getLinearVelocity()*timeStep;
|
||||
|
||||
vmVector3 newCenter = centerW + diffvec;
|
||||
vmVector3 aabbMin = minPerElem(newCenter - halfW,centerW - halfW);
|
||||
vmVector3 aabbMax = maxPerElem(newCenter + halfW,centerW + halfW);
|
||||
|
||||
centerW = 0.5f * (aabbMin + aabbMax);
|
||||
halfW =0.5f * (aabbMax - aabbMin);
|
||||
|
||||
center[0] = centerW[0];
|
||||
center[1] = centerW[1];
|
||||
center[2] = centerW[2];
|
||||
|
||||
half[0] = halfW[0];
|
||||
half[1] = halfW[1];
|
||||
half[2] = halfW[2];
|
||||
}
|
||||
|
||||
inline
|
||||
void TrbState::reset()
|
||||
{
|
||||
#if 0
|
||||
mSleepCount = 0;
|
||||
mMotionType = PfxMotionTypeActive;
|
||||
mDeleted = 0;
|
||||
mSleeping = 0;
|
||||
mUseSleep = 1;
|
||||
mUseCcd = 0;
|
||||
mUseContactCallback = 0;
|
||||
mUseSleepCallback = 0;
|
||||
mRigidBodyId = 0;
|
||||
mContactFilterSelf = 0xffffffff;
|
||||
mContactFilterTarget = 0xffffffff;
|
||||
mLinearDamping = 1.0f;
|
||||
mAngularDamping = 0.99f;
|
||||
mPosition = vmVector3(0.0f);
|
||||
mOrientation = vmQuat::identity();
|
||||
mLinearVelocity = vmVector3(0.0f);
|
||||
mAngularVelocity = vmVector3(0.0f);
|
||||
#endif
|
||||
|
||||
setMotionType(PfxMotionTypeActive);
|
||||
contactFilterSelf=contactFilterTarget=0xffffffff;
|
||||
deleted = 0;
|
||||
mSleeping = 0;
|
||||
useSleep = 1;
|
||||
trbBodyIdx=0;
|
||||
mSleepCount=0;
|
||||
useCcd = 0;
|
||||
useContactCallback = 0;
|
||||
useSleepCallback = 0;
|
||||
linearDamping = 1.0f;
|
||||
angularDamping = 0.99f;
|
||||
}
|
||||
|
||||
#endif /* __TRBSTATEVEC_H__ */
|
||||
|
||||
Reference in New Issue
Block a user