mirror of
https://github.com/copyrighttxt/watrbx-game-engine.git
synced 2026-09-04 20:57:49 +00:00
fahhh
This commit is contained in:
watrabi
@@ -0,0 +1,415 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "V8Kernel/KernelIndex.h"
|
||||
#include "V8Kernel/Cofm.h"
|
||||
#include "V8Kernel/SimBody.h"
|
||||
#include "V8Kernel/Link.h"
|
||||
#include "Util/IndexArray.h"
|
||||
#include "Util/IndexedTree.h"
|
||||
#include "Util/Memory.h"
|
||||
#include "rbx/threadsafe.h"
|
||||
|
||||
class btCollisionObject;
|
||||
|
||||
namespace RBX {
|
||||
|
||||
/*
|
||||
Body class. Handles rigid joints and kinematic/dynamic joints. Automatically calculates
|
||||
mass properties. Automatically updates state.
|
||||
|
||||
StateRoot: Body or Joint that contains the latest state id.
|
||||
RigidRoot: Body that I am clumped with
|
||||
COFM: Calculated for every body in a rigid group - chain is broken for linked bodies
|
||||
SIMBody: Only Free Bodies have this
|
||||
|
||||
Whenever adjusting a joint, must increment the stateRoot of the assembly
|
||||
|
||||
World Body // Special Body - no COFM
|
||||
| |
|
||||
| -- Body // anchored body - rigid join to the world body - when moving, adjust state index
|
||||
|
|
||||
-- Body // anchored body - rigid join to the world body
|
||||
|
|
||||
-- Body
|
||||
|
|
||||
-- Body
|
||||
0 // this is a link
|
||||
-- Body // this is the StateRoot for the chain below it
|
||||
|
|
||||
Body
|
||||
|
||||
Body (free) // this is the StateRoot for the free body and chain
|
||||
| // this body has group COFM for the top three bodies + SimBody
|
||||
-- Body
|
||||
|
|
||||
-- Body
|
||||
0
|
||||
-- Body // this body has a group COFM for the bodies below it
|
||||
|
|
||||
Body
|
||||
|
||||
COFM: body, kinematic, assembly (assumes a floating object)
|
||||
|
||||
*/
|
||||
|
||||
class BodyPvSetter;
|
||||
|
||||
class Body : public IndexedTree
|
||||
, public Allocator<Body>
|
||||
{
|
||||
public:
|
||||
friend class KernelData;
|
||||
friend class Kernel;
|
||||
friend class SimBody;
|
||||
|
||||
private:
|
||||
rbx::spin_mutex mutex; // for safe calls that require update
|
||||
|
||||
// Unique identifier set by the World
|
||||
boost::uint64_t uid;
|
||||
int guidIndex;
|
||||
|
||||
int leafBodyIndex;
|
||||
|
||||
int& getLeafBodyIndex() {return leafBodyIndex;}
|
||||
|
||||
int connectorUseCount; // how many connectors connect this body
|
||||
static Body* worldBody;
|
||||
static void initStaticData(); // inits the world body
|
||||
|
||||
Body* root; // top body - either anchored or 6 dof
|
||||
|
||||
Cofm* cofm;
|
||||
Cofm* getCofm() {return cofm;} // use these to insure const correctness;
|
||||
|
||||
SimBody* simBody; // Only present for parent && in kernel
|
||||
SimBody* getSimBody() {return simBody;}
|
||||
const SimBody* getConstSimBody() const {return simBody;}
|
||||
|
||||
void refreshCofm();
|
||||
|
||||
Link* link; // if link != NULL, use for "getMeInParent()"
|
||||
|
||||
// defining variables
|
||||
bool canThrottle; // this body can throttle - i.e., slow down
|
||||
CoordinateFrame meInParent; // used if no link, i.e. - rigid connection
|
||||
Matrix3 moment;
|
||||
float mass;
|
||||
Vector3 cofmOffset;
|
||||
|
||||
// resulting variables
|
||||
unsigned int stateIndex;
|
||||
PV pv;
|
||||
|
||||
void resetRoot(Body* newRoot);
|
||||
|
||||
bool validateParentCofmDirty();
|
||||
|
||||
const CoordinateFrame& getMeInParent() {
|
||||
RBXASSERT(getParent());
|
||||
return getLink() ? getLink()->getChildInParent() : meInParent;
|
||||
}
|
||||
const CoordinateFrame& getConstMeInParent() const {
|
||||
RBXASSERT(getConstParent());
|
||||
RBXASSERT(!getConstLink()); // fails if linked (i.e. only works in same clump);
|
||||
return meInParent;
|
||||
}
|
||||
|
||||
void updatePV(); // Does not inline anyhow.
|
||||
|
||||
bool pvIsUpToDate() const {
|
||||
if (!getConstParent()) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
if (stateIndex != getRoot()->getStateIndexNoUpdate()) {
|
||||
return false;
|
||||
}
|
||||
else {
|
||||
return getConstParent()->pvIsUpToDate();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void onChildAdded(Body* newChild);
|
||||
void onChildRemoved(Body* newChild);
|
||||
|
||||
Body* calcRoot() {return getParent() ? getParent()->calcRoot() : this;}
|
||||
const Body* calcRootConst() const {return getConstParent() ? getConstParent()->calcRootConst() : this;}
|
||||
|
||||
///////////////////////////////////////////////
|
||||
// Indexed Tree
|
||||
/*override*/ void onParentChanging();
|
||||
/*override*/ void onParentChanged(IndexedTree* oldParent);
|
||||
/*override*/ void onChildAdding(IndexedTree* child);
|
||||
/*override*/ void onChildAdded(IndexedTree* child);
|
||||
/*override*/ void onChildRemoved(IndexedTree* child);
|
||||
|
||||
public:
|
||||
Body();
|
||||
|
||||
~Body();
|
||||
|
||||
static unsigned int getNextStateIndex();
|
||||
|
||||
// Static world body
|
||||
static Body* getWorldBody();
|
||||
|
||||
SimBody* getRootSimBody() {return getRoot()->getSimBody();}
|
||||
const SimBody* getConstRootSimBody() const {return getRoot()->getConstSimBody();}
|
||||
|
||||
void setUID( boost::uint64_t _uid );
|
||||
boost::uint64_t getUID() const { return uid; }
|
||||
|
||||
// Used only by the solver inspector to id the objects between different clients
|
||||
void setGuidIndex( int _guidIndex ) { guidIndex = _guidIndex; }
|
||||
int getGuidIndex() const { return guidIndex; }
|
||||
|
||||
//////////////////////////////////////////////////////
|
||||
// From Cofm, SimBody
|
||||
bool cofmIsClean() {return getCofm() ? !getCofm()->getIsDirty() : true;}
|
||||
|
||||
//////////////////////////////////////////////////////
|
||||
// From Child
|
||||
void makeCofmDirty();
|
||||
|
||||
void advanceStateIndex();
|
||||
|
||||
void makeStateDirty() {
|
||||
getRoot()->advanceStateIndex();
|
||||
}
|
||||
|
||||
unsigned int getStateIndex() {
|
||||
updatePV();
|
||||
return stateIndex;
|
||||
}
|
||||
|
||||
unsigned int getStateIndexNoUpdate() const {
|
||||
return stateIndex;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////
|
||||
// Base vs. Branch
|
||||
|
||||
Body* getChild(int i) {return getTypedChild<Body>(i);}
|
||||
const Body* getConstChild(int i) const {return getConstTypedChild<Body>(i);}
|
||||
|
||||
Body* getParent() {return getTypedParent<Body>();}
|
||||
const Body* getConstParent() const {return getConstTypedParent<Body>();}
|
||||
|
||||
Link* getLink() {return link;}
|
||||
const Link* getConstLink() const {return link;}
|
||||
|
||||
const Body* getRoot() const {
|
||||
RBXASSERT_SLOW(root == calcRootConst());
|
||||
return root;
|
||||
}
|
||||
|
||||
Body* getRoot() {
|
||||
RBXASSERT_SLOW(root == calcRootConst());
|
||||
return root;
|
||||
}
|
||||
|
||||
const Vector3& getCofmOffset() {
|
||||
return cofmOffset;
|
||||
}
|
||||
|
||||
const Vector3& getBranchCofmOffset();
|
||||
|
||||
// Only works on bodies in same clump - otherwise not const for the link and will assert
|
||||
CoordinateFrame getMeInAncestor(const Body* ancestor) const {
|
||||
if (ancestor == this) {
|
||||
return CoordinateFrame();
|
||||
}
|
||||
else if (ancestor == getConstParent()) {
|
||||
return getConstMeInParent();
|
||||
}
|
||||
else {
|
||||
return getConstParent()->getMeInAncestor(ancestor) * getConstMeInParent();
|
||||
}
|
||||
}
|
||||
|
||||
inline float getMass() const {return mass;}
|
||||
inline Matrix3 getIBody() const {return moment;}
|
||||
inline Vector3 getIBodyV3() const {return Math::toDiagonal(getIBody());}
|
||||
Matrix3 getIBodyAtPoint(const Vector3& point);
|
||||
inline Matrix3 getMoment() const {return getIBody();}
|
||||
inline Vector3 getPrincipalMoment() const {return getIBodyV3();}
|
||||
Matrix3 getIWorld() {return Math::momentToWorldSpace(getIBody(), getCoordinateFrame().rotation);}
|
||||
Matrix3 getIWorldAtPoint(const Vector3& point);
|
||||
|
||||
// Branch refers to everything below me....
|
||||
float getBranchMass() {return getCofm() ? getCofm()->getMass() : mass;}
|
||||
Matrix3 getBranchIBody() {return getCofm() ? getCofm()->getMoment() : moment;}
|
||||
Vector3 getBranchIBodyV3() {return Math::toDiagonal(getBranchIBody());}
|
||||
Matrix3 getBranchIWorld() {return Math::momentToWorldSpace(getBranchIBody(), getCoordinateFrame().rotation);}
|
||||
Matrix3 getBranchIWorldAtPoint(const Vector3& point);
|
||||
Vector3 getBranchCofmPos();
|
||||
CoordinateFrame getBranchCofmCoordinateFrame();
|
||||
|
||||
//
|
||||
|
||||
const PV& getPvFast() const {
|
||||
RBXASSERT_FISHING(pvIsUpToDate());
|
||||
return pv;
|
||||
}
|
||||
|
||||
// Current Job should hold the Data Model write lock or somehow have locked the Body::mutex.
|
||||
const PV& getPvUnsafe() {
|
||||
updatePV();
|
||||
return pv;
|
||||
}
|
||||
|
||||
const PV& getPV_Spin_Lock() {
|
||||
rbx::spin_mutex::scoped_lock lock(mutex);
|
||||
updatePV();
|
||||
return pv;
|
||||
}
|
||||
|
||||
const PV& getPvSafe() const {
|
||||
Body* thisNotConst = const_cast<Body*>(this);
|
||||
return thisNotConst->getPV_Spin_Lock();
|
||||
}
|
||||
|
||||
const Vector3& getPosFast() const {
|
||||
RBXASSERT_FISHING(pvIsUpToDate());
|
||||
return pv.position.translation;
|
||||
}
|
||||
|
||||
const Vector3& getPos() {
|
||||
updatePV();
|
||||
return pv.position.translation;
|
||||
}
|
||||
|
||||
const CoordinateFrame& getCoordinateFrameFast() const {
|
||||
RBXASSERT_FISHING(pvIsUpToDate());
|
||||
return pv.position;
|
||||
}
|
||||
|
||||
const CoordinateFrame& getCoordinateFrame() {
|
||||
updatePV();
|
||||
return pv.position;
|
||||
}
|
||||
|
||||
const Velocity& getVelocity() {
|
||||
updatePV();
|
||||
return pv.velocity;
|
||||
}
|
||||
|
||||
bool getCanThrottle() const {
|
||||
return canThrottle;
|
||||
}
|
||||
|
||||
void accumulateImpulseAtBranchCofm(const Vector3& impulse) {
|
||||
if (SimBody* s = getRootSimBody()) {
|
||||
s->accumulateImpulseAtBranchCofm(impulse);
|
||||
}
|
||||
}
|
||||
|
||||
void accumulateLinearImpulse(const Vector3& impulse, const Vector3& worldPos) {
|
||||
if (SimBody* s = getRootSimBody()) {
|
||||
s->accumulateImpulse(impulse, worldPos);
|
||||
}
|
||||
}
|
||||
|
||||
void accumulateRotationalImpulse(const Vector3& impulse) {
|
||||
if (SimBody* s = getRootSimBody()) {
|
||||
s->accumulateRotationalImpulse(impulse);
|
||||
}
|
||||
}
|
||||
|
||||
void accumulateForceAtBranchCofm(const Vector3& force) {
|
||||
RBXASSERT(getRoot() == this); // should only be called on Root objects
|
||||
if (SimBody* s = getRootSimBody()) {
|
||||
s->accumulateForceCofm(force);
|
||||
}
|
||||
}
|
||||
|
||||
void accumulateForce(const Vector3& force, const Vector3& worldPos) {
|
||||
if (SimBody* s = getRootSimBody()) {
|
||||
s->accumulateForce(force, worldPos);
|
||||
}
|
||||
}
|
||||
|
||||
void accumulateTorque(const Vector3& torque) {
|
||||
if (SimBody* s = getRootSimBody()) {
|
||||
s->accumulateTorque(torque);
|
||||
}
|
||||
}
|
||||
|
||||
void resetForceAccumulators() {
|
||||
if (SimBody* s = getRootSimBody()) {
|
||||
s->resetForceAccumulators();
|
||||
}
|
||||
}
|
||||
|
||||
void resetImpulseAccumulators() {
|
||||
if (SimBody* s = getRootSimBody()) {
|
||||
s->resetImpulseAccumulators();
|
||||
}
|
||||
}
|
||||
|
||||
const Vector3& getBranchForce() const {
|
||||
RBXASSERT(getRoot() == this); // should only be called on Root objects
|
||||
const SimBody* s = getConstRootSimBody();
|
||||
return s ? s->getForce() : Vector3::zero();
|
||||
}
|
||||
|
||||
const Vector3& getBranchTorque() const {
|
||||
RBXASSERT(getRoot() == this); // should only be called on Root objects
|
||||
const SimBody* s = getConstRootSimBody();
|
||||
return s ? s->getTorque() : Vector3::zero();
|
||||
}
|
||||
|
||||
const Velocity& getBranchVelocity() { // velocity at the COFM of the assembly
|
||||
RBXASSERT(getRoot() == this); // should only be called on Root objects
|
||||
const SimBody* s = getRootSimBody();
|
||||
return s ? s->getPV().velocity : Velocity::zero();
|
||||
}
|
||||
|
||||
/////////////////////////////////////////////////////
|
||||
// setting properties
|
||||
//
|
||||
void setParent(Body* parent) {setIndexedTreeParent(parent);}
|
||||
|
||||
void setMeInParent(const CoordinateFrame& _meInParent);
|
||||
|
||||
void setMeInParent(Link* _link);
|
||||
|
||||
void setMass(float _mass);
|
||||
|
||||
void setMoment(const Matrix3& _momentInBody);
|
||||
|
||||
void setCofmOffset(const Vector3& _centerOfMassInBody);
|
||||
|
||||
|
||||
// Only Primitive can set these - make sure we keep byproducts updated - fuzzy extents, etc.
|
||||
// ToDo: Hack - is there a better way to limit access to only primitives, while not including "friend class Primitive"???
|
||||
|
||||
void setPv(const PV& _pv, const BodyPvSetter& bpv);
|
||||
|
||||
void setCoordinateFrame(const CoordinateFrame& worldCoord, const BodyPvSetter& bpv);
|
||||
|
||||
void setVelocity(const Velocity& worldVelocity, const BodyPvSetter& bpv);
|
||||
|
||||
void setCanThrottle(bool value, const BodyPvSetter& bpv);
|
||||
|
||||
void updateBulletCollisionObject(btCollisionObject* object);
|
||||
|
||||
public:
|
||||
|
||||
/////////////////////////////////////////////////////////
|
||||
// Debugging / reporting functions / complex stuff
|
||||
//
|
||||
|
||||
inline bool isLeafBody() const {return leafBodyIndex >= 0;}
|
||||
|
||||
float kineticEnergy();
|
||||
float potentialEnergy();
|
||||
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
@@ -0,0 +1,13 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace RBX {
|
||||
|
||||
// Stub class - primitive descends from this to protect body::setCoordinateFrame() from others
|
||||
class BodyPvSetter
|
||||
{
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
@@ -0,0 +1,111 @@
|
||||
/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "V8Kernel/ContactParams.h"
|
||||
#include "V8Kernel/PolyConnectors.h"
|
||||
#include "Util/G3DCore.h"
|
||||
#include "rbx/Debug.h"
|
||||
|
||||
#include "BulletCollision/NarrowphaseCollision/btPersistentManifold.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionDispatcher.h"
|
||||
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
|
||||
#include "btBulletCollisionCommon.h"
|
||||
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class BulletShapeConnector : public PolyConnector,
|
||||
public Allocator<BulletShapeConnector>
|
||||
{
|
||||
protected:
|
||||
btCollisionObject* bulletCollisionObject0;
|
||||
btCollisionObject* bulletCollisionObject1;
|
||||
btCollisionAlgorithm* bulletAlgo;
|
||||
int bulletManifoldIndex;
|
||||
int bulletPointCacheIndex;
|
||||
|
||||
void updateConnectorPointFromManifold(bool refreshContacts = true);
|
||||
void realignConnectorsToBulletContacts();
|
||||
bool foundValidContactPointFromBulletManifold(btPersistentManifold* man, Vector3& p0World, Vector3& p1World);
|
||||
|
||||
private:
|
||||
/*override*/ GeoPairType getConnectorType() const {return BULLET_SHAPE_CONNECTOR;}
|
||||
bool validObjectCFrames();
|
||||
virtual void updateBulletCollisionObjects();
|
||||
|
||||
public:
|
||||
BulletShapeConnector(
|
||||
Body* b0,
|
||||
Body* b1,
|
||||
const ContactParams& contactParams,
|
||||
btCollisionObject* bulletColObj0,
|
||||
btCollisionObject* bulletColObj1,
|
||||
btCollisionAlgorithm* algo,
|
||||
int manifoldIndex,
|
||||
int cacheIndex
|
||||
)
|
||||
: PolyConnector(b0, b1, contactParams, 0, 0)
|
||||
, bulletCollisionObject0(bulletColObj0)
|
||||
, bulletCollisionObject1(bulletColObj1)
|
||||
, bulletAlgo(algo)
|
||||
, bulletManifoldIndex(manifoldIndex)
|
||||
, bulletPointCacheIndex(cacheIndex)
|
||||
{
|
||||
}
|
||||
|
||||
~BulletShapeConnector();
|
||||
|
||||
/*override*/ void updateContactPoint();
|
||||
void findValidContactAfterNarrowphase();
|
||||
bool recalculateValidPoints(btManifoldArray& btManArray, Vector3& pt0InWorld, Vector3& pt1InWorld);
|
||||
void setBulletManifoldPointIndex(int index) { bulletPointCacheIndex = index; }
|
||||
int getBulletManifoldIndex(void) { return bulletManifoldIndex;}
|
||||
int getBulletPointCacheIndex(void) { return bulletPointCacheIndex;}
|
||||
|
||||
void refreshIndividualPoint(bool swapped, Vector3 pt0InWorld, Vector3 pt1InWorld, btManifoldArray& manArray);
|
||||
void updatePointWithTransform(bool swapped, btManifoldPoint& manifoldPoint);
|
||||
bool isPointInvalid( btManifoldPoint& manifoldPoint, double validThreshold);
|
||||
|
||||
static bool match(BulletShapeConnector* oldCon, BulletShapeConnector* newCon)
|
||||
{
|
||||
return ((oldCon->bulletManifoldIndex == newCon->bulletManifoldIndex)
|
||||
&& (oldCon->bulletPointCacheIndex == newCon->bulletPointCacheIndex)
|
||||
&& (oldCon->getConnectorType() == newCon->getConnectorType()));
|
||||
}
|
||||
};
|
||||
|
||||
class BulletShapeCellConnector : public BulletShapeConnector
|
||||
{
|
||||
private:
|
||||
/*override*/ GeoPairType getConnectorType() const {return BULLET_SHAPE_CELL_CONNECTOR;}
|
||||
/*override*/ void updateBulletCollisionObjects();
|
||||
|
||||
|
||||
public:
|
||||
BulletShapeCellConnector(
|
||||
Body* b0,
|
||||
Body* b1,
|
||||
const ContactParams& contactParams,
|
||||
btCollisionObject* bulletColObj0,
|
||||
btCollisionObject* bulletColObj1,
|
||||
btCollisionAlgorithm* algo,
|
||||
int manifoldIndex,
|
||||
int cacheIndex
|
||||
)
|
||||
: BulletShapeConnector(b0, b1, contactParams, bulletColObj0, bulletColObj1, algo, manifoldIndex, cacheIndex)
|
||||
{
|
||||
}
|
||||
~BulletShapeCellConnector() {}
|
||||
|
||||
/*override*/ void updateContactPoint();
|
||||
|
||||
static bool match(BulletShapeCellConnector* oldCon, BulletShapeCellConnector* newCon)
|
||||
{
|
||||
return ((oldCon->bulletManifoldIndex == newCon->bulletManifoldIndex)
|
||||
&& (oldCon->bulletPointCacheIndex == newCon->bulletPointCacheIndex)
|
||||
&& (oldCon->getConnectorType() == newCon->getConnectorType()));
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace RBX
|
||||
@@ -0,0 +1,36 @@
|
||||
#pragma once
|
||||
|
||||
#include "v8kernel/ContactConnector.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class BuoyancyConnector : public RBX::ContactConnector
|
||||
{
|
||||
private:
|
||||
Vector3 position; // force application point in object space
|
||||
Vector3 force;
|
||||
Vector3 torque;
|
||||
|
||||
float floatDistance;
|
||||
float sinkDistance;
|
||||
float submergeRatio;
|
||||
|
||||
protected:
|
||||
/*override*/ void computeForce( bool throttling );
|
||||
/*override*/ virtual KernelType getConnectorKernelType() const { return Connector::BUOYANCY; }
|
||||
|
||||
public:
|
||||
void updateContactPoint(); // Only for debug rendering now
|
||||
|
||||
const Vector3& getPosition() { return position; }
|
||||
const Vector3 getWorldPosition();
|
||||
void setForce( const Vector3& f ) { force = f; }
|
||||
void setTorque( const Vector3& t ) { torque = t; }
|
||||
void getWaterBand( float& up, float& down ) { up = floatDistance; down = sinkDistance; }
|
||||
void setWaterBand( const float& up, const float& down ) { floatDistance = up; sinkDistance = down; }
|
||||
float getSubMergeRatio() { return submergeRatio; }
|
||||
void setSubMergeRatio( const float& ratio ) { submergeRatio = ratio; }
|
||||
|
||||
BuoyancyConnector(Body* b0, Body* b1, const Vector3& pos);
|
||||
};
|
||||
}
|
||||
@@ -0,0 +1,48 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Util/G3DCore.h"
|
||||
#include "Util/Memory.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class Body;
|
||||
|
||||
class Cofm : public Allocator<Cofm>
|
||||
{
|
||||
private:
|
||||
Body* body;
|
||||
bool dirty;
|
||||
Vector3 cofmInBody;
|
||||
float mass;
|
||||
Matrix3 moment;
|
||||
|
||||
void updateIfDirty(); // true if was dirty
|
||||
|
||||
public:
|
||||
Cofm(Body* body);
|
||||
|
||||
bool getIsDirty() const {return dirty;}
|
||||
|
||||
void makeDirty() {
|
||||
dirty = true;
|
||||
}
|
||||
|
||||
const Vector3& getCofmInBody() {
|
||||
updateIfDirty();
|
||||
return cofmInBody;
|
||||
}
|
||||
|
||||
float getMass() {
|
||||
updateIfDirty();
|
||||
return mass;
|
||||
}
|
||||
|
||||
const Matrix3& getMoment() {
|
||||
updateIfDirty();
|
||||
return moment;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,218 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Util/NormalID.h"
|
||||
#include "rbx/Debug.h"
|
||||
#include "Util/Memory.h"
|
||||
#include "Util/Math.h"
|
||||
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class Point;
|
||||
class Body;
|
||||
class Kernel;
|
||||
|
||||
class RBXBaseClass Connector
|
||||
{
|
||||
friend class KernelData;
|
||||
friend class Kernel;
|
||||
private:
|
||||
int humanoidIndex;
|
||||
int realTimeIndex;
|
||||
int secondPassIndex;
|
||||
int jointIndex;
|
||||
int buoyancyIndex;
|
||||
int contactIndex;
|
||||
|
||||
protected:
|
||||
// Used by kernel only. Only add types that KernelData.h can handle.
|
||||
typedef enum {
|
||||
CONTACT,
|
||||
JOINT,
|
||||
HUMANOID,
|
||||
KERNEL_JOINT,
|
||||
BUOYANCY
|
||||
} KernelType;
|
||||
|
||||
/*implement*/ virtual KernelType getConnectorKernelType() const = 0;
|
||||
|
||||
public:
|
||||
|
||||
int& getHumanoidIndex() {return humanoidIndex;}
|
||||
int& getRealTimeIndex() {return realTimeIndex;}
|
||||
int& getSecondPassIndex() {return secondPassIndex;}
|
||||
int& getJointIndex() {return jointIndex;}
|
||||
int& getBuoyancyIndex() {return buoyancyIndex;}
|
||||
int& getContactIndex() {return contactIndex;}
|
||||
bool isHumanoid() {return humanoidIndex >= 0;}
|
||||
bool isRealTime() {return realTimeIndex >= 0;}
|
||||
bool isSecondPass() {return secondPassIndex >= 0;}
|
||||
bool isJoint() {return jointIndex >= 0;}
|
||||
bool isBuoyancy() {return buoyancyIndex >= 0;}
|
||||
bool isContact() {return contactIndex >= 0;}
|
||||
bool isInKernel() {return isHumanoid() || isRealTime() || isSecondPass() || isJoint() || isBuoyancy() || isContact();}
|
||||
|
||||
Connector() : humanoidIndex(-1), realTimeIndex(-1), secondPassIndex(-1),
|
||||
jointIndex(-1), buoyancyIndex(-1), contactIndex(-1) {}
|
||||
virtual ~Connector() {}
|
||||
|
||||
virtual bool computeCanThrottle();
|
||||
|
||||
/////////// Called by kernel //////////////////////////////
|
||||
virtual void computeForce(bool throttling) = 0;
|
||||
virtual bool computeImpulse(float& residualVelocity) {return false;}
|
||||
virtual bool getBroken() {return false;}
|
||||
|
||||
typedef enum { body0, body1 } BodyIndex;
|
||||
virtual Body* getBody(BodyIndex id) = 0;
|
||||
|
||||
// DEBUGGING
|
||||
virtual float potentialEnergy() {return 0.0;}
|
||||
};
|
||||
|
||||
class JointConnector
|
||||
: public Connector
|
||||
{
|
||||
protected:
|
||||
/*override*/ virtual KernelType getConnectorKernelType() const {return JOINT;}
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// Force = kForce * d_length
|
||||
// Torque = kTorque * d_angle
|
||||
// d_length = d_angle * L;
|
||||
// d_angle= d_length / L
|
||||
// This should produce an equivalent "force" at a length L from the center.
|
||||
// so, force at a distance of L is Force = L * torque;
|
||||
// F = kTorque * d_angle / L = kForce * d_L
|
||||
// kTorque = kForce * d_L * L * L / d_L = kForce * L * L;
|
||||
|
||||
class RotateConnector
|
||||
: public JointConnector
|
||||
{
|
||||
private:
|
||||
float baseRotation; // rotation when assembled
|
||||
|
||||
protected:
|
||||
Body* b0;
|
||||
Body* b1;
|
||||
CoordinateFrame j0;
|
||||
CoordinateFrame j1;
|
||||
|
||||
float k; // spring constant
|
||||
// Integrator properties
|
||||
float currentAngle;
|
||||
float desiredAngle;
|
||||
float increment;
|
||||
bool zeroVelocity;
|
||||
|
||||
float computeNormalRotation(Vector3& normal);
|
||||
|
||||
float computeNormalRotationFromBase(Vector3& normal);
|
||||
float computeNormalRotationFromBaseFast(Vector3& normal);
|
||||
|
||||
virtual void stepGoals();
|
||||
|
||||
/*override*/ Body* getBody(BodyIndex id);
|
||||
|
||||
/////////// Called by kernel //////////////////////////////
|
||||
/*override*/ virtual void computeForce(bool throttling);
|
||||
|
||||
public:
|
||||
RotateConnector(
|
||||
Body* _b0,
|
||||
Body* _b1,
|
||||
const CoordinateFrame& _j0,
|
||||
const CoordinateFrame& _j1,
|
||||
float _baseAngle,
|
||||
float kValue,
|
||||
float armLength);
|
||||
|
||||
void reset(); // after networking receive - update to synch internal desiredRotation
|
||||
|
||||
void setRotationalGoal(float rotationalGoal);
|
||||
|
||||
void setVelocityGoal(float velocity);
|
||||
|
||||
static float computeJointAngle(
|
||||
const CoordinateFrame& b0,
|
||||
const CoordinateFrame& b1,
|
||||
const CoordinateFrame& j0,
|
||||
const CoordinateFrame& j1,
|
||||
Vector3& normal);
|
||||
};
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class PointToPointBreakConnector
|
||||
: public JointConnector
|
||||
{
|
||||
protected:
|
||||
Point* point0;
|
||||
Point* point1;
|
||||
float k; // spring constant
|
||||
float breakForce;
|
||||
|
||||
// state variable
|
||||
bool broken;
|
||||
|
||||
void forceToPoints(const G3D::Vector3& force);
|
||||
|
||||
/*override*/ Body* getBody(BodyIndex id);
|
||||
|
||||
public:
|
||||
// initialize
|
||||
PointToPointBreakConnector(Point* point0, Point* point1, float k, float breakForce) :
|
||||
point0(point0),
|
||||
point1(point1),
|
||||
k(k),
|
||||
breakForce(breakForce),
|
||||
broken(false)
|
||||
{}
|
||||
|
||||
/////////// Called by kernel //////////////////////////////
|
||||
/*override*/ virtual void computeForce(bool throttling);
|
||||
|
||||
/*override*/ virtual bool getBroken() { return broken; }
|
||||
|
||||
/* override */ virtual float potentialEnergy();
|
||||
|
||||
///////// for breakage - one "Joint" may need to break all connectors
|
||||
inline void setBroken() { broken = true; }
|
||||
|
||||
float getStiffness() const { return k; }
|
||||
void setStiffness( float value ) { k = value; }
|
||||
};
|
||||
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
|
||||
// TODO: update normal very infrequently....
|
||||
|
||||
class NormalBreakConnector
|
||||
: public PointToPointBreakConnector
|
||||
, public Allocator<NormalBreakConnector>
|
||||
{
|
||||
private:
|
||||
NormalId normalIdBody0;
|
||||
|
||||
public:
|
||||
NormalBreakConnector(
|
||||
Point* point0,
|
||||
Point* point1,
|
||||
float k,
|
||||
float breakForce,
|
||||
NormalId normalIdBody0)
|
||||
: PointToPointBreakConnector(point0, point1, k, breakForce)
|
||||
, normalIdBody0(normalIdBody0)
|
||||
{}
|
||||
|
||||
/////////// Called by kernel //////////////////////////////
|
||||
/*override*/ virtual void computeForce(bool throttling);
|
||||
};
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,62 @@
|
||||
/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Util/G3DCore.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class Constants {
|
||||
private:
|
||||
static const int JOINT_FORCE_DATA = 7;
|
||||
static const float MAX_LEGO_JOINT_FORCES_THEORY[JOINT_FORCE_DATA];
|
||||
static const float MAX_LEGO_JOINT_FORCES_MEASURED[JOINT_FORCE_DATA];
|
||||
|
||||
static float LEGO_GRID_MASS(); // kg
|
||||
static float LEGO_JOINT_K(); // kg/s^2
|
||||
static float LEGO_DEFAULT_ELASTIC_K();
|
||||
|
||||
static float unitJointK();
|
||||
|
||||
static float getJointKMultiplier(const Vector3& clippedSortedSize, bool ball);
|
||||
|
||||
Constants();
|
||||
|
||||
public:
|
||||
///////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Timestep related stuff
|
||||
//
|
||||
static int uiStepsPerSec() {return longUiStepsPerSec() * 2;}
|
||||
static int worldStepsPerUiStep();
|
||||
static int longUiStepsPerSec() {return 30;}
|
||||
static int worldStepsPerLongUiStep();
|
||||
static int kernelStepsPerWorldStep();
|
||||
static int freeFallStepsPerWorldStep();
|
||||
static int worldStepsPerSec();
|
||||
static int kernelStepsPerSec();
|
||||
static int kernelStepsPerUiStep();
|
||||
static int freeFallStepsPerSec();
|
||||
static int impulseSolverMaxIterations();
|
||||
static float impulseSolverAccuracy();
|
||||
static int impulseSolverAccuracyScalar();
|
||||
static float impulseSolverSymStateTorqueBound();
|
||||
static float impulseSolverSymStateForceBound();
|
||||
static float uiDt();
|
||||
static float longUiStepDt();
|
||||
static float worldDt();
|
||||
static float kernelDt();
|
||||
static float freeFallDt();
|
||||
static const Vector3& denormalSmall();
|
||||
|
||||
//////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Dimenensions and K related stuff
|
||||
//
|
||||
static inline float getKmsGravity() {return -9.81f;}
|
||||
static float getKmsMaxJointForce(float grid1, float grid2);
|
||||
static float getElasticMultiplier(float elasticity);
|
||||
static float getJointK(const Vector3& size, bool ball); // kg/s^2
|
||||
};
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,220 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "V8Kernel/Connector.h"
|
||||
#include "V8Kernel/ContactParams.h"
|
||||
#include "V8Kernel/Pair.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
#include "v8kernel/SimBody.h"
|
||||
#include "v8kernel/Constants.h"
|
||||
#include "Util/NormalID.h"
|
||||
#include "rbx/Debug.h"
|
||||
#include "Util/Memory.h"
|
||||
|
||||
|
||||
namespace RBX {
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////
|
||||
class ContactConnector : public Connector
|
||||
{
|
||||
private:
|
||||
static int inContactHit;
|
||||
static int outOfContactHit;
|
||||
|
||||
int age;
|
||||
// Cache for computeImpulse
|
||||
Matrix3 deltaVelPerUnitImpulse;
|
||||
Matrix3 impulsePerUnitDeltaVel;
|
||||
float inverseMass;
|
||||
float penetrationVelocity;
|
||||
float reboundVelocity;
|
||||
bool impulseComputed;
|
||||
|
||||
protected:
|
||||
GeoPair geoPair;
|
||||
ContactParams contactParams;
|
||||
PairParams oldContactPoint;
|
||||
PairParams contactPoint;
|
||||
|
||||
// state variables
|
||||
float firstApproach;
|
||||
float threshold;
|
||||
|
||||
// delay variables
|
||||
float forceMagLast; // contact only variable
|
||||
Vector3 frictionOffset;
|
||||
|
||||
/*override*/ virtual KernelType getConnectorKernelType() const { return Connector::CONTACT; }
|
||||
|
||||
public:
|
||||
virtual void updateContactPoint();
|
||||
|
||||
static float overlapGoal() {return 0.01f;} // standard goal seek for overlapping objects
|
||||
|
||||
/*override*/ Body* getBody(BodyIndex id) {return (id == body0) ? geoPair.body0 : geoPair.body1;}
|
||||
void setBody(int id, Body* b) {
|
||||
if (id == 0) {
|
||||
geoPair.body0 = b;
|
||||
}
|
||||
else {
|
||||
geoPair.body1 = b;
|
||||
}
|
||||
}
|
||||
|
||||
ContactConnector(Body* b0, Body* b1, const ContactParams& contactParams)
|
||||
: contactParams(contactParams), inverseMass(0.0f), impulseComputed(false)
|
||||
{
|
||||
geoPair.body0 = b0;
|
||||
geoPair.body1 = b1;
|
||||
reset();
|
||||
}
|
||||
|
||||
void reset() { // cleans up state variables for buffered version
|
||||
firstApproach = 0.0;
|
||||
threshold = 0.0;
|
||||
forceMagLast = 0.0;
|
||||
age = 0;
|
||||
penetrationVelocity = 0.0;
|
||||
reboundVelocity = 0.0;
|
||||
}
|
||||
|
||||
inline void clearImpulseComputed() { impulseComputed = false; }
|
||||
|
||||
bool isIntersecting() {
|
||||
RBXASSERT(geoPair.geoPairType == POINT_PLANE_PAIR);
|
||||
return (contactPoint.length < -overlapGoal());
|
||||
}
|
||||
|
||||
// Reorder the SimBody(s) so that simBody0 is always in kernel and adjust contact point data accordingly
|
||||
bool getReordedSimBody(SimBody*& simBody0, SimBody*& simBody1, Body*& bodyNotInKernel, PairParams& params);
|
||||
bool getReordedSimBody(SimBody*& simBody0, SimBody*& simBody1, PairParams& params);
|
||||
|
||||
// Compute the relative velocities between the two bodies
|
||||
bool getSimBodyAndContactVelocity(SimBody*& simBody0, SimBody*& simBody1, PairParams& params,
|
||||
float& normalVel, Vector3& perpVel);
|
||||
float computeRelativeVelocity(const PairParams ¶ms, Vector3* deltaVnormal, Vector3* perpVel);
|
||||
float computeRelativeVelocity();
|
||||
|
||||
void applyContactPointForSymmetryDetection(SimBody* simBody0, SimBody* simBody1,
|
||||
const PairParams& params, float direction);
|
||||
|
||||
const ContactParams& getContactParams() const { return contactParams; }
|
||||
void setContactParams(const ContactParams& params) { contactParams = params; }
|
||||
|
||||
/////////// Called by kernel //////////////////////////////
|
||||
/* override*/ virtual void computeForce(bool throttling);
|
||||
/* override*/ virtual bool computeImpulse(float& residualVelocity);
|
||||
/* override*/ bool canThrottle() const;
|
||||
|
||||
// Debug
|
||||
static float percentActive();
|
||||
|
||||
float computeOverlap() { // positive == bigger overlap
|
||||
updateContactPoint();
|
||||
return -contactPoint.length;
|
||||
}
|
||||
|
||||
inline PairParams& getContactPoint() { return contactPoint; }
|
||||
inline const PairParams& getContactPoint() const { return contactPoint; }
|
||||
|
||||
void getLengthNormalPosition(Vector3& position, Vector3& normal, float& length) {
|
||||
position = contactPoint.position;
|
||||
normal = contactPoint.normal;
|
||||
length = contactPoint.length;
|
||||
}
|
||||
|
||||
inline bool isRestingContact() { return age > 4; }
|
||||
};
|
||||
|
||||
class GeoPairConnector
|
||||
: public ContactConnector
|
||||
, public Allocator<GeoPairConnector>
|
||||
{
|
||||
public:
|
||||
GeoPairConnector(Body* b0, Body* b1, const ContactParams& contactParams) : ContactConnector(b0, b1, contactParams)
|
||||
{}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/*override*/ void updateContactPoint()
|
||||
{
|
||||
geoPair.computeLengthNormalPosition(contactPoint);
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
void setPointPlane(const Vector3* oPoint, const Vector3* oPlane,
|
||||
int pointId, NormalId planeId)
|
||||
{
|
||||
geoPair.setPointPlane(oPoint, oPlane, pointId, planeId);
|
||||
}
|
||||
|
||||
void setEdgeEdgePlane(const Vector3* e0, const Vector3* e1,
|
||||
NormalId n0, NormalId n1, NormalId planeId, float edgeLength0, float edgeLength1)
|
||||
{
|
||||
geoPair.setEdgeEdgePlane(e0, e1, n0, n1, planeId, edgeLength0, edgeLength1);
|
||||
}
|
||||
|
||||
void setEdgeEdge(const Vector3* e0, const Vector3* e1, NormalId n0, NormalId n1)
|
||||
{
|
||||
geoPair.setEdgeEdge(e0, e1, n0, n1);
|
||||
}
|
||||
|
||||
bool match(Body* b0, Body* b1, GeoPairType pairType, int param0, int param1)
|
||||
{
|
||||
return geoPair.match(b0, b1, pairType, param0, param1);
|
||||
}
|
||||
};
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class BallBallConnector : public ContactConnector
|
||||
, public Allocator<BallBallConnector>
|
||||
{
|
||||
private:
|
||||
float radius0;
|
||||
float radiusSum;
|
||||
|
||||
public:
|
||||
BallBallConnector(Body* b0, Body* b1, const ContactParams& contactParams)
|
||||
: ContactConnector(b0, b1, contactParams)
|
||||
{}
|
||||
|
||||
/*override*/ void updateContactPoint();
|
||||
void setRadius(float r0, float r1) {
|
||||
radius0 = r0;
|
||||
radiusSum = r0 + r1;
|
||||
}
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class BallBlockConnector : public ContactConnector,
|
||||
public Allocator<BallBlockConnector>
|
||||
{
|
||||
private:
|
||||
float radius0; // ball
|
||||
Vector3 offset1;
|
||||
NormalId normalId1;
|
||||
GeoPairType geoPairType;
|
||||
|
||||
void computeBallPoint(PairParams& params);
|
||||
void computeBallEdge(PairParams& params);
|
||||
void computeBallPlane(PairParams& params);
|
||||
|
||||
public:
|
||||
BallBlockConnector(Body* b0, Body* b1, const ContactParams& contactParams)
|
||||
: ContactConnector(b0, b1, contactParams)
|
||||
{}
|
||||
|
||||
/*override*/ void updateContactPoint();
|
||||
void setBallBlock(float _radius0, const Vector3* _offset1, RBX::NormalId _normalID, GeoPairType _geoPairType) {
|
||||
offset1 = *_offset1;
|
||||
radius0 = _radius0;
|
||||
normalId1 = _normalID;
|
||||
geoPairType = _geoPairType;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,44 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class ContactParams {
|
||||
public:
|
||||
float kSpring; // spring constant
|
||||
float kNeg; // elastic - spring constant on bounceback
|
||||
float kFriction; // contact only variable stored as true value * -0.5;???
|
||||
float kElasticity;
|
||||
|
||||
ContactParams()
|
||||
: kSpring(0.0)
|
||||
, kFriction(0.0)
|
||||
, kNeg(0.0)
|
||||
, kElasticity(0.0f)
|
||||
|
||||
{}
|
||||
};
|
||||
|
||||
|
||||
enum GeoPairType { BALL_POINT_PAIR, // BALL to BLOCK
|
||||
BALL_EDGE_PAIR,
|
||||
BALL_PLANE_PAIR,
|
||||
// BLOCK to BLOCK
|
||||
POINT_PLANE_PAIR,
|
||||
EDGE_EDGE_PLANE_PAIR, // two edges, plane needed to supply normal
|
||||
EDGE_EDGE_PAIR, // two edges, guaranteed to be overlapping
|
||||
|
||||
VERTEX_PLANE_CONNECTOR,
|
||||
EDGE_EDGE_CONNECTOR,
|
||||
EDGE_EDGE_PLANE_CONNECTOR,
|
||||
|
||||
BALL_VERTEX_CONNECTOR,
|
||||
BALL_EDGE_CONNECTOR,
|
||||
BALL_PLANE_CONNECTOR,
|
||||
|
||||
BULLET_SHAPE_CONNECTOR,
|
||||
BULLET_SHAPE_CELL_CONNECTOR };
|
||||
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,13 @@
|
||||
#pragma once
|
||||
|
||||
#include "RBX/Debug.h"
|
||||
|
||||
// Engine assertions often cause the game to stop running.
|
||||
// Until we can fix these, turn them off.
|
||||
//#define RBX_DEBUGENGINE
|
||||
|
||||
#ifdef RBX_DEBUGENGINE
|
||||
#define RBX_ENGINE_ASSERT(expr) RBXASSERT(expr)
|
||||
#else
|
||||
#define RBX_ENGINE_ASSERT(expr) ((void)0)
|
||||
#endif
|
||||
@@ -0,0 +1,74 @@
|
||||
/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "rbx/Debug.h"
|
||||
#include "Util/G3DCore.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class Kernel;
|
||||
|
||||
class RBXBaseClass IStage {
|
||||
public:
|
||||
typedef enum { CLEAN_STAGE,
|
||||
JOINT_STAGE,
|
||||
GROUND_STAGE,
|
||||
EDGE_STAGE,
|
||||
CONTACT_STAGE,
|
||||
TREE_STAGE,
|
||||
MOVING_STAGE,
|
||||
SPATIAL_FILTER,
|
||||
MECH_TO_ASSEMBLY_STAGE,
|
||||
ASSEMBLY_STAGE,
|
||||
MOVING_ASSEMBLY_STAGE,
|
||||
STEP_JOINTS_STAGE,
|
||||
HUMANOID_STAGE,
|
||||
SLEEP_STAGE,
|
||||
SIMULATE_STAGE,
|
||||
KERNEL_STAGE} StageType;
|
||||
|
||||
private:
|
||||
IStage* upstream;
|
||||
IStage* downstream;
|
||||
|
||||
const IStage* findStageImpl(StageType stageType) const {
|
||||
const IStage* answer = this;
|
||||
while (answer->getStageType() != stageType) {
|
||||
answer = answer->getDownstream();
|
||||
}
|
||||
return answer;
|
||||
}
|
||||
|
||||
public:
|
||||
IStage(IStage* upstream, IStage* downstream)
|
||||
: upstream(upstream), downstream(downstream)
|
||||
{}
|
||||
|
||||
virtual ~IStage() {
|
||||
if (downstream) {
|
||||
delete downstream;
|
||||
}
|
||||
}
|
||||
|
||||
IStage* getUpstream() {return upstream;}
|
||||
IStage* getDownstream() {return downstream;}
|
||||
const IStage* getDownstream() const {return downstream;}
|
||||
|
||||
virtual StageType getStageType() const = 0;
|
||||
|
||||
const IStage* findStage(StageType stageType) const {
|
||||
return findStageImpl(stageType);
|
||||
}
|
||||
|
||||
IStage* findStage(StageType stageType) {
|
||||
return const_cast<IStage*>(findStageImpl(stageType));
|
||||
}
|
||||
|
||||
virtual Kernel* getKernel() {
|
||||
RBXASSERT(downstream);
|
||||
return downstream->getKernel();
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,128 @@
|
||||
#pragma once
|
||||
|
||||
#include "V8Kernel/IStage.h"
|
||||
#include "V8Kernel/BodyPvSetter.h"
|
||||
#include "boost/scoped_ptr.hpp"
|
||||
#include "solver/Solver.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
namespace Profiling
|
||||
{
|
||||
class CodeProfiler;
|
||||
}
|
||||
|
||||
class Connector;
|
||||
class Body;
|
||||
class Point;
|
||||
class KernelData;
|
||||
|
||||
class Kernel : public IStage,
|
||||
public BodyPvSetter
|
||||
{
|
||||
private:
|
||||
static int numKernels;
|
||||
int maxBodies;
|
||||
bool inStepCode;
|
||||
int numLastIterations;
|
||||
int numOfMaxIterations;
|
||||
float error;
|
||||
float maxError;
|
||||
bool validateBody(Body* b);
|
||||
bool validateConnector(Connector* connector) const;
|
||||
bool validateConnectorBody(Body* b) const;
|
||||
|
||||
KernelData* kernelData;
|
||||
|
||||
// Funny Physics
|
||||
void stepWorldFunnyPhysics(int worldStepId);
|
||||
void stepFunnyPhysics(const Vector3& move);
|
||||
void stepFunnyPhysicsBody(Body* b, const Vector3& move);
|
||||
|
||||
Point* searchForDuplicatePoint(Point* tempPoint);
|
||||
|
||||
void preStep();
|
||||
void preStepThrottled();
|
||||
void stepWorld( boost::uint64_t distDebugTime );
|
||||
void stepWorldThrottled( boost::uint64_t debugTime );
|
||||
|
||||
bool usingPGSSolver;
|
||||
|
||||
public:
|
||||
PGSSolver pgsSolver;
|
||||
Kernel(IStage* upstream);
|
||||
~Kernel();
|
||||
|
||||
///////////////////////////////////////////
|
||||
// IStage
|
||||
/*override*/ IStage::StageType getStageType() const {return IStage::KERNEL_STAGE;}
|
||||
|
||||
/*override*/ Kernel* getKernel() {return this;}
|
||||
|
||||
void step(bool throttling, int numThreads, boost::uint64_t debugTime);
|
||||
|
||||
void insertBody(Body* b);
|
||||
void insertPoint(Point* p);
|
||||
void insertConnector(Connector* c);
|
||||
|
||||
void removeBody(Body* b);
|
||||
void removePoint(Point* p);
|
||||
void removeConnector(Connector* c);
|
||||
|
||||
// double up on points if same body, position....
|
||||
// TODO: move this to the point class - reference counted pointer
|
||||
//
|
||||
Point* newPointLocal(class Body* _body, const Vector3& worldPos);
|
||||
Point* newPoint(class Body* _body, const Vector3& worldPos);
|
||||
void deletePoint(class Point* point);
|
||||
|
||||
////////////////////////////////////////////////////////
|
||||
//
|
||||
// Debugging Stuff
|
||||
//
|
||||
void report(); // system energy to log file
|
||||
static void reportMemorySizes();
|
||||
|
||||
float connectorSpringEnergy() const;
|
||||
float bodyPotentialEnergy() const;
|
||||
float bodyKineticEnergy() const;
|
||||
float totalEnergy() const {
|
||||
return connectorSpringEnergy() + bodyPotentialEnergy() + bodyKineticEnergy();
|
||||
}
|
||||
float totalKineticEnergy() const {
|
||||
return connectorSpringEnergy() + bodyKineticEnergy();
|
||||
}
|
||||
|
||||
int numFreeFallBodies() const;
|
||||
int numRealTimeBodies() const;
|
||||
int numJointBodies() const;
|
||||
int numContactBodies() const;
|
||||
int numBodies() const {return numFreeFallBodies() + numRealTimeBodies() + numJointBodies() + numContactBodies();}
|
||||
int numBodiesMax() const {return maxBodies;}
|
||||
int numLeafBodies() const;
|
||||
int numPoints() const;
|
||||
int numConnectors() const;
|
||||
int numHumanoidConnectors() const;
|
||||
int numRealTimeConnectors() const;
|
||||
int numSecondPassConnectors() const;
|
||||
int numJointConnectors() const;
|
||||
int numBuoyancyConnectors() const;
|
||||
int numContactConnectors() const;
|
||||
|
||||
inline int numIterations() const {return numLastIterations;}
|
||||
inline int numMaxIterations() const {return numOfMaxIterations;}
|
||||
inline float getSolverError() const {return error;}
|
||||
inline float getMaxSolverError() const {return maxError;}
|
||||
int fakeDeceptiveSolverIterations() const;
|
||||
int fakeDeceptiveMatrixSize() const;
|
||||
|
||||
///////////////////////////////////////////
|
||||
// Profiler
|
||||
boost::scoped_ptr<Profiling::CodeProfiler> profilingKernelBodies;
|
||||
boost::scoped_ptr<Profiling::CodeProfiler> profilingKernelConnectors;
|
||||
|
||||
void setUsingPGSSolver(bool pgsOn) { usingPGSSolver = pgsOn; }
|
||||
bool getUsingPGSSolver() const { return usingPGSSolver; }
|
||||
void dumpLog( bool enable ) { pgsSolver.dumpLog( enable ); }
|
||||
};
|
||||
} // namespace
|
||||
@@ -0,0 +1,380 @@
|
||||
#pragma once
|
||||
|
||||
#include "v8kernel/Body.h"
|
||||
#include "V8Kernel/SimBody.h"
|
||||
#include "V8Kernel/Point.h"
|
||||
#include "V8Kernel/Connector.h"
|
||||
#include "V8Kernel/ContactConnector.h"
|
||||
#include "V8Kernel/BuoyancyConnector.h"
|
||||
#include "V8kernel/Constants.h"
|
||||
#include "V8datamodel/FastLogSettings.h"
|
||||
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class KernelData {
|
||||
public:
|
||||
// main object types
|
||||
IndexArray<SimBody, &SimBody::getFreeFallBodyIndex> freeFallBodies; // bodies with no connectors
|
||||
IndexArray<SimBody, &SimBody::getRealTimeBodyIndex> realTimeBodies; // humanoid bodies that are not throttle-able
|
||||
IndexArray<SimBody, &SimBody::getJointBodyIndex> jointBodies; // bodies with joint connectors
|
||||
IndexArray<SimBody, &SimBody::getContactBodyIndex> contactBodies; // bodies with contact connectors but no joint connectors
|
||||
IndexArray<Body, &Body::getLeafBodyIndex> leafBodies; // need update PV every step, NOT in kernel!
|
||||
IndexArray<Point, &Point::getKernelIndex> points;
|
||||
IndexArray<Connector, &Connector::getHumanoidIndex> humanoidConnectors; // The humanoids
|
||||
IndexArray<Connector, &Connector::getSecondPassIndex> secondPassConnectors; // kernel joints
|
||||
IndexArray<Connector, &Connector::getRealTimeIndex> realTimeConnectors; // connectors on humanoid body parts
|
||||
IndexArray<Connector, &Connector::getJointIndex> jointConnectors;
|
||||
IndexArray<Connector, &Connector::getBuoyancyIndex> buoyancyConnectors;
|
||||
IndexArray<Connector, &Connector::getContactIndex> contactConnectors;
|
||||
|
||||
KernelData()
|
||||
{
|
||||
}
|
||||
|
||||
~KernelData() {
|
||||
RBXASSERT(freeFallBodies.size() == 0);
|
||||
RBXASSERT(realTimeBodies.size() == 0);
|
||||
RBXASSERT(jointBodies.size() == 0);
|
||||
RBXASSERT(contactBodies.size() == 0);
|
||||
RBXASSERT(leafBodies.size() == 0);
|
||||
RBXASSERT(points.size() == 0);
|
||||
RBXASSERT(humanoidConnectors.size() == 0);
|
||||
RBXASSERT(secondPassConnectors.size() == 0);
|
||||
RBXASSERT(realTimeConnectors.size() == 0);
|
||||
RBXASSERT(jointConnectors.size() == 0);
|
||||
RBXASSERT(buoyancyConnectors.size() == 0);
|
||||
RBXASSERT(contactConnectors.size() == 0);
|
||||
}
|
||||
|
||||
inline void addLeafBodies(Body* b)
|
||||
{
|
||||
for (int i = 0; i < b->numChildren(); ++i)
|
||||
{
|
||||
Body* child = b->getChild(i);
|
||||
RBXASSERT(!child->isLeafBody());
|
||||
RBXASSERT(child != child->getRoot());
|
||||
if (child->connectorUseCount > 0)
|
||||
{
|
||||
addLeafBody(child);
|
||||
}
|
||||
addLeafBodies(child);
|
||||
}
|
||||
}
|
||||
|
||||
inline void insertBody(Body* b)
|
||||
{
|
||||
RBXASSERT(b->getRoot() == b);
|
||||
SimBody* simBody = b->getSimBody();
|
||||
RBXASSERT(!b->isLeafBody() && !simBody->isInKernel());
|
||||
addBodyToNewList(simBody);
|
||||
RBXASSERT(simBody->validateBodyLists());
|
||||
}
|
||||
|
||||
inline void removeBody(Body* b)
|
||||
{
|
||||
RBXASSERT(b->getRoot() == b);
|
||||
RBXASSERT(!b->isLeafBody());
|
||||
|
||||
SimBody* simBody = b->getSimBody();
|
||||
removeBodyFromCurrentList(simBody);
|
||||
simBody->clearSymStateAndAccummulator();
|
||||
RBXASSERT(simBody->validateBodyLists());
|
||||
}
|
||||
|
||||
inline void addConnector(Connector* c, bool pgsOn)
|
||||
{
|
||||
RBXASSERT(!c->isInKernel());
|
||||
Body* body0 = c->getBody(Connector::body0);
|
||||
Body* body1 = c->getBody(Connector::body1);
|
||||
SimBody* simBody0 = body0 ? body0->getRootSimBody() : NULL;
|
||||
SimBody* simBody1 = body1 ? body1->getRootSimBody() : NULL;
|
||||
|
||||
Connector::KernelType connectorType = c->getConnectorKernelType();
|
||||
if ((simBody0 == NULL || !simBody0->isInKernel()) &&
|
||||
(simBody1 == NULL || !simBody1->isInKernel()) &&
|
||||
connectorType != Connector::HUMANOID &&
|
||||
connectorType != Connector::JOINT &&
|
||||
connectorType != Connector::KERNEL_JOINT)
|
||||
return;
|
||||
|
||||
if (connectorType == Connector::HUMANOID)
|
||||
{
|
||||
humanoidConnectors.fastAppend(c);
|
||||
if (simBody0)
|
||||
simBody0->incrementHumanoidConnectorCount();
|
||||
if (simBody1)
|
||||
simBody1->incrementHumanoidConnectorCount();
|
||||
}
|
||||
else if (connectorType == Connector::KERNEL_JOINT)
|
||||
{
|
||||
secondPassConnectors.fastAppend(c);
|
||||
if (simBody0)
|
||||
simBody0->incrementSecondPassConnectorCount();
|
||||
if (simBody1)
|
||||
simBody1->incrementSecondPassConnectorCount();
|
||||
}
|
||||
else if (pgsOn && connectorType == Connector::JOINT)
|
||||
{
|
||||
jointConnectors.fastAppend(c);
|
||||
if (simBody0)
|
||||
simBody0->incrementJointConnetorCount();
|
||||
if (simBody1)
|
||||
simBody1->incrementJointConnetorCount();
|
||||
}
|
||||
else if (pgsOn && connectorType == Connector::BUOYANCY)
|
||||
{
|
||||
buoyancyConnectors.fastAppend(c);
|
||||
if (simBody0)
|
||||
simBody0->incrementBuoyancyConnectorCount();
|
||||
if (simBody1)
|
||||
simBody1->incrementBuoyancyConnectorCount();
|
||||
}
|
||||
else if ((simBody0 && !simBody0->getBody()->getCanThrottle()) ||
|
||||
(simBody1 && !simBody1->getBody()->getCanThrottle()))
|
||||
{
|
||||
realTimeConnectors.fastAppend(c);
|
||||
if (simBody0)
|
||||
simBody0->incrementRealTimeConnectorCount();
|
||||
if (simBody1)
|
||||
simBody1->incrementRealTimeConnectorCount();
|
||||
}
|
||||
else if (!pgsOn &&
|
||||
(connectorType == Connector::JOINT ||
|
||||
connectorType == Connector::BUOYANCY ||
|
||||
((simBody0 && simBody0->isJointBody()) || // Contact connectors in touch with joint bodies
|
||||
(simBody1 && simBody1->isJointBody())))) // are considered joint connectors.
|
||||
{
|
||||
jointConnectors.fastAppend(c);
|
||||
if (simBody0)
|
||||
simBody0->incrementJointConnetorCount();
|
||||
if (simBody1)
|
||||
simBody1->incrementJointConnetorCount();
|
||||
}
|
||||
else
|
||||
{
|
||||
RBXASSERT(connectorType == Connector::CONTACT);
|
||||
contactConnectors.fastAppend(c);
|
||||
if (simBody0)
|
||||
simBody0->incrementContactConnectorCount();
|
||||
if (simBody1)
|
||||
simBody1->incrementContactConnectorCount();
|
||||
}
|
||||
|
||||
if (body0)
|
||||
addConnectorToBody(c, body0);
|
||||
if (body1)
|
||||
addConnectorToBody(c, body1);
|
||||
|
||||
RBXASSERT(simBody0 == NULL || simBody0->validateBodyLists());
|
||||
RBXASSERT(simBody1 == NULL || simBody1->validateBodyLists());
|
||||
}
|
||||
|
||||
inline void removeConnector(Connector* c)
|
||||
{
|
||||
if (!c->isInKernel())
|
||||
return;
|
||||
|
||||
Body* body0 = c->getBody(Connector::body0);
|
||||
Body* body1 = c->getBody(Connector::body1);
|
||||
SimBody* simBody0 = body0 ? body0->getRootSimBody() : NULL;
|
||||
SimBody* simBody1 = body1 ? body1->getRootSimBody() : NULL;
|
||||
|
||||
if (c->isHumanoid())
|
||||
{
|
||||
humanoidConnectors.fastRemove(c);
|
||||
if (simBody0)
|
||||
simBody0->decrementHumanoidConnectorCount();
|
||||
if (simBody1)
|
||||
simBody1->decrementHumanoidConnectorCount();
|
||||
}
|
||||
else if (c->isSecondPass())
|
||||
{
|
||||
secondPassConnectors.fastRemove(c);
|
||||
if (simBody0)
|
||||
simBody0->decrementSecondPassConnectorCount();
|
||||
if (simBody1)
|
||||
simBody1->decrementSecondPassConnectorCount();
|
||||
}
|
||||
else if (c->isRealTime())
|
||||
{
|
||||
realTimeConnectors.fastRemove(c);
|
||||
if (simBody0)
|
||||
simBody0->decrementRealTimeConnectorCount();
|
||||
if (simBody1)
|
||||
simBody1->decrementRealTimeConnectorCount();
|
||||
}
|
||||
else if (c->isJoint())
|
||||
{
|
||||
jointConnectors.fastRemove(c);
|
||||
if (simBody0)
|
||||
simBody0->decrementJointConnetorCount();
|
||||
if (simBody1)
|
||||
simBody1->decrementJointConnetorCount();
|
||||
}
|
||||
else if (c->isBuoyancy())
|
||||
{
|
||||
buoyancyConnectors.fastRemove(c);
|
||||
if (simBody0)
|
||||
simBody0->decrementBuoyancyConnectorCount();
|
||||
if (simBody1)
|
||||
simBody1->decrementBuoyancyConnectorCount();
|
||||
}
|
||||
else
|
||||
{
|
||||
RBXASSERT(c->isContact());
|
||||
contactConnectors.fastRemove(c);
|
||||
if (simBody0)
|
||||
simBody0->decrementContactConnectorCount();
|
||||
if (simBody1)
|
||||
simBody1->decrementContactConnectorCount();
|
||||
|
||||
ContactConnector* conn = static_cast<ContactConnector*>(c);
|
||||
PairParams params = conn->getContactPoint();
|
||||
if (conn->getReordedSimBody(simBody0, simBody1, params))
|
||||
conn->applyContactPointForSymmetryDetection(simBody0, simBody1, params, -1.0f);
|
||||
}
|
||||
|
||||
if (body0)
|
||||
removeConnectorFromBody(c, body0);
|
||||
if (body1)
|
||||
removeConnectorFromBody(c, body1);
|
||||
|
||||
RBXASSERT(simBody0 == NULL || simBody0->validateBodyLists());
|
||||
RBXASSERT(simBody1 == NULL || simBody1->validateBodyLists());
|
||||
}
|
||||
|
||||
private:
|
||||
|
||||
inline void addLeafBody(Body* b)
|
||||
{
|
||||
RBXASSERT(!b->isLeafBody());
|
||||
RBXASSERT(b->connectorUseCount > 0);
|
||||
leafBodies.fastAppend(b);
|
||||
RBXASSERT(b->isLeafBody());
|
||||
}
|
||||
|
||||
inline void removeLeafBody(Body* b)
|
||||
{
|
||||
RBXASSERT(b->isLeafBody());
|
||||
leafBodies.fastRemove(b);
|
||||
RBXASSERT(!b->isLeafBody());
|
||||
}
|
||||
|
||||
inline void removeLeafBodies(Body* b)
|
||||
{
|
||||
for (int i = 0; i < b->numChildren(); ++i)
|
||||
{
|
||||
Body* child = b->getChild(i);
|
||||
if (child->isLeafBody())
|
||||
{
|
||||
removeLeafBody(child);
|
||||
}
|
||||
removeLeafBodies(child);
|
||||
}
|
||||
}
|
||||
|
||||
inline void removeBodyFromCurrentList(SimBody* simBody)
|
||||
{
|
||||
if (simBody->isRealTimeBody())
|
||||
{
|
||||
removeLeafBodies(simBody->getBody());
|
||||
realTimeBodies.fastRemove(simBody);
|
||||
} else if (simBody->isJointBody())
|
||||
{
|
||||
removeLeafBodies(simBody->getBody());
|
||||
jointBodies.fastRemove(simBody);
|
||||
} else if (simBody->isContactBody())
|
||||
{
|
||||
contactBodies.fastRemove(simBody);
|
||||
} else if (simBody->isFreeFallBody())
|
||||
{
|
||||
freeFallBodies.fastRemove(simBody);
|
||||
simBody->updateAngMomentum();
|
||||
} else
|
||||
return;
|
||||
simBody->setDt(0.0f);
|
||||
}
|
||||
|
||||
inline void addBodyToNewList(SimBody* simBody)
|
||||
{
|
||||
if (!simBody->getBody()->getCanThrottle())
|
||||
{
|
||||
if (!simBody->isRealTimeBody())
|
||||
{
|
||||
removeBodyFromCurrentList(simBody);
|
||||
realTimeBodies.fastAppend(simBody);
|
||||
simBody->setDt(Constants::kernelDt());
|
||||
addLeafBodies(simBody->getBody());
|
||||
}
|
||||
} else if (simBody->getJointConnectorCount() > 0)
|
||||
{
|
||||
if (!simBody->isJointBody())
|
||||
{
|
||||
removeBodyFromCurrentList(simBody);
|
||||
jointBodies.fastAppend(simBody);
|
||||
simBody->setDt(Constants::kernelDt());
|
||||
addLeafBodies(simBody->getBody());
|
||||
}
|
||||
} else if (simBody->getContactConnectorCount() > 0)
|
||||
{
|
||||
if (!simBody->isContactBody())
|
||||
{
|
||||
removeBodyFromCurrentList(simBody);
|
||||
contactBodies.fastAppend(simBody);
|
||||
simBody->setDt(Constants::freeFallDt());
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (simBody->getConnectorCount() == 0)
|
||||
{
|
||||
if (!simBody->isFreeFallBody())
|
||||
{
|
||||
removeBodyFromCurrentList(simBody);
|
||||
freeFallBodies.fastAppend(simBody);
|
||||
simBody->setDt(Constants::freeFallDt());
|
||||
simBody->clearSymStateAndAccummulator();
|
||||
}
|
||||
} else
|
||||
{
|
||||
if (!simBody->isJointBody())
|
||||
{
|
||||
removeBodyFromCurrentList(simBody);
|
||||
jointBodies.fastAppend(simBody);
|
||||
simBody->setDt(Constants::kernelDt());
|
||||
addLeafBodies(simBody->getBody());
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
inline void addConnectorToBody(Connector* c, Body* body)
|
||||
{
|
||||
body->connectorUseCount++;
|
||||
SimBody* simBody = body->getRootSimBody();
|
||||
|
||||
if (!simBody->isInKernel())
|
||||
return;
|
||||
addBodyToNewList(simBody);
|
||||
|
||||
// adds leaf bodies if root is already here and it is subject to the spring solver
|
||||
if (body != body->getRoot() && !body->isLeafBody() &&
|
||||
(simBody->isJointBody() || simBody->isRealTimeBody()))
|
||||
addLeafBody(body);
|
||||
}
|
||||
|
||||
inline void removeConnectorFromBody(Connector* c, Body* body)
|
||||
{
|
||||
body->connectorUseCount--;
|
||||
|
||||
if (body->isLeafBody() && body->connectorUseCount == 0)
|
||||
removeLeafBody(body);
|
||||
|
||||
SimBody* simBody = body->getRootSimBody();
|
||||
if (!simBody->isInKernel())
|
||||
return;
|
||||
addBodyToNewList(simBody);
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,26 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "rbx/Debug.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class KernelIndex {
|
||||
protected:
|
||||
int kernelIndex;
|
||||
|
||||
public:
|
||||
bool indexInKernel() const {
|
||||
return (kernelIndex != -1);
|
||||
}
|
||||
|
||||
KernelIndex() : kernelIndex(-1)
|
||||
{}
|
||||
|
||||
~KernelIndex() {
|
||||
RBXASSERT(!indexInKernel());
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,86 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Util/G3DCore.h"
|
||||
#include "rbx/Declarations.h"
|
||||
#include "Util/Memory.h"
|
||||
#include "Util/Math.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class Body;
|
||||
|
||||
class RBXBaseClass Link
|
||||
{
|
||||
friend class Body;
|
||||
|
||||
protected:
|
||||
Body* body; // body I'm affilliated with (child)
|
||||
CoordinateFrame parentCoord;
|
||||
CoordinateFrame childCoord;
|
||||
CoordinateFrame childCoordInverse;
|
||||
|
||||
CoordinateFrame childInParent;
|
||||
unsigned int stateIndex;
|
||||
|
||||
virtual void computeChildInParent(CoordinateFrame& answer) const = 0;
|
||||
|
||||
void dirty();
|
||||
|
||||
void setBody(Body* b) {body = b;}
|
||||
|
||||
public:
|
||||
Link();
|
||||
|
||||
~Link();
|
||||
|
||||
const CoordinateFrame& getChildInParent();
|
||||
|
||||
Body* getBody() const {return body;}
|
||||
|
||||
void reset(
|
||||
const CoordinateFrame& parentC,
|
||||
const CoordinateFrame& childC);
|
||||
};
|
||||
|
||||
|
||||
class RevoluteLink
|
||||
: public Link
|
||||
, public Allocator<RevoluteLink>
|
||||
{
|
||||
private:
|
||||
float jointAngle;
|
||||
|
||||
/*override*/ void computeChildInParent(CoordinateFrame& answer) const;
|
||||
|
||||
public:
|
||||
RevoluteLink() : jointAngle(0.0f)
|
||||
{
|
||||
}
|
||||
|
||||
void setJointAngle(float value) {
|
||||
jointAngle = value;
|
||||
dirty();
|
||||
}
|
||||
};
|
||||
|
||||
class D6Link
|
||||
: public Link
|
||||
, public Allocator<D6Link>
|
||||
{
|
||||
private:
|
||||
CoordinateFrame offsetCFrame;
|
||||
|
||||
/*override*/ void computeChildInParent(CoordinateFrame& answer) const;
|
||||
|
||||
public:
|
||||
void setJointOffsetCFrame(const CoordinateFrame& value) {
|
||||
offsetCFrame = value;
|
||||
RBXASSERT(!Math::hasNanOrInf(value));
|
||||
dirty();
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
@@ -0,0 +1,148 @@
|
||||
/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "V8Kernel/ContactParams.h"
|
||||
#include "Util/NormalID.h"
|
||||
#include "Util/G3DCore.h"
|
||||
#include "rbx/Debug.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class Body;
|
||||
|
||||
class PairParams {
|
||||
public:
|
||||
Vector3 normal;
|
||||
union {
|
||||
float length;
|
||||
float rotation;
|
||||
};
|
||||
Vector3 position;
|
||||
PairParams() {
|
||||
normal = position = Vector3::zero();
|
||||
length = 0.0f;
|
||||
}
|
||||
bool operator==(const PairParams& other) {
|
||||
return (length == other.length && position == other.position && normal == other.normal);
|
||||
}
|
||||
};
|
||||
|
||||
///////////////////////////////////////////////////////////////////
|
||||
|
||||
class GeoPair
|
||||
{
|
||||
public:
|
||||
GeoPairType geoPairType;
|
||||
|
||||
// note, pair point0 and point1 have the following polarity
|
||||
// ball ball: radius0 == point0 body
|
||||
// ball block: ball->point0, block->point1
|
||||
// point plane: pointBlock->0, planeBlock->1
|
||||
// edge edge plane: planeBlock->1
|
||||
// fixed, not allocated here
|
||||
|
||||
// This is defining data
|
||||
Vector3 offset0;
|
||||
Vector3 offset1;
|
||||
Body* body0;
|
||||
Body* body1;
|
||||
float edgeLength0;
|
||||
float edgeLength1;
|
||||
struct {
|
||||
union {
|
||||
RBX::NormalId normalID0;
|
||||
float radius0; };
|
||||
union {
|
||||
RBX::NormalId normalID1;
|
||||
float radiusSum; };
|
||||
union {
|
||||
RBX::NormalId planeID;// edge/edge/plane coords - the normal from the plane
|
||||
int point0ID; };
|
||||
} pairData;
|
||||
|
||||
private:
|
||||
void computePointPlane(PairParams& _params);
|
||||
void computeEdgeEdgePlane(PairParams& _params);
|
||||
void computeEdgeEdgePlane2(PairParams& _params);
|
||||
void computeEdgeEdge(PairParams& _params);
|
||||
|
||||
public:
|
||||
GeoPair();
|
||||
|
||||
////////// Kernel Update
|
||||
|
||||
inline void computeLengthNormalPosition(PairParams& _params)
|
||||
{
|
||||
switch (geoPairType) {
|
||||
case (POINT_PLANE_PAIR): computePointPlane(_params); break;
|
||||
case (EDGE_EDGE_PLANE_PAIR): computeEdgeEdgePlane2(_params); break;
|
||||
case (EDGE_EDGE_PAIR): computeEdgeEdge(_params); break;
|
||||
default: RBXASSERT(0);
|
||||
}
|
||||
}
|
||||
|
||||
///////////////// GeoPair geometric functions
|
||||
|
||||
void setPointPlane(const Vector3* _offsetPoint,
|
||||
const Vector3* _offsetPlane, int _pointID, RBX::NormalId _planeNormalID) {
|
||||
offset0 = *_offsetPoint;
|
||||
offset1 = *_offsetPlane;
|
||||
pairData.point0ID = _pointID; // purely here for the match
|
||||
pairData.normalID1 = _planeNormalID;
|
||||
geoPairType = POINT_PLANE_PAIR;
|
||||
}
|
||||
|
||||
void setEdgeEdgePlane(const Vector3* _edge0, const Vector3* _edge1,
|
||||
RBX::NormalId _normal0, RBX::NormalId _normal1, RBX::NormalId _planeID, float _edgeLength0, float _edgeLength1) {
|
||||
offset0 = *_edge0;
|
||||
offset1 = *_edge1;
|
||||
pairData.normalID0 = _normal0;
|
||||
pairData.normalID1 = _normal1;
|
||||
pairData.planeID = _planeID;
|
||||
edgeLength0 = _edgeLength0;
|
||||
edgeLength1 = _edgeLength1;
|
||||
geoPairType = EDGE_EDGE_PLANE_PAIR;
|
||||
}
|
||||
|
||||
void setEdgeEdge(const Vector3* _edge0, const Vector3* _edge1,
|
||||
RBX::NormalId _normal0, RBX::NormalId _normal1) {
|
||||
offset0 = *_edge0;
|
||||
offset1 = *_edge1;
|
||||
pairData.normalID0 = _normal0;
|
||||
pairData.normalID1 = _normal1;
|
||||
geoPairType = EDGE_EDGE_PAIR;
|
||||
}
|
||||
|
||||
bool match(Body* _b0, Body* _b1, GeoPairType _pairType, int param0, int param1) {
|
||||
if (_pairType == POINT_PLANE_PAIR) {
|
||||
return ( (_b0 == body0)
|
||||
&& (_b1 == body1)
|
||||
&& (param0 == pairData.point0ID)
|
||||
&& (param1 == pairData.normalID1) );
|
||||
}
|
||||
|
||||
else if (_pairType == EDGE_EDGE_PLANE_PAIR) {
|
||||
return ( (_b0 == body0)
|
||||
&& (_b1 == body1)
|
||||
&& (param0 == pairData.normalID0)
|
||||
&& (param1 == pairData.normalID1) );
|
||||
}
|
||||
|
||||
else {
|
||||
RBXASSERT(_pairType == EDGE_EDGE_PAIR);
|
||||
return ( ( (_b0 == body0)
|
||||
&& (_b1 == body1)
|
||||
&& (param0 == pairData.normalID0)
|
||||
&& (param1 == pairData.normalID1) )
|
||||
||
|
||||
( (_b0 == body1)
|
||||
&& (_b1 == body0)
|
||||
&& (param0 == pairData.normalID1)
|
||||
&& (param1 == pairData.normalID0) )
|
||||
);
|
||||
}
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace RBX
|
||||
@@ -0,0 +1,83 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "V8Kernel/KernelIndex.h"
|
||||
#include "Util/G3DCore.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class Body;
|
||||
|
||||
|
||||
class Point
|
||||
: public KernelIndex
|
||||
{
|
||||
friend class KernelData;
|
||||
friend class Kernel;
|
||||
private:
|
||||
int& getKernelIndex() {return kernelIndex;}
|
||||
int numOwners;
|
||||
|
||||
protected:
|
||||
Body* body;
|
||||
|
||||
// constant
|
||||
Vector3 localPos;
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
// auxillary variables, computed on every frame
|
||||
Vector3 worldPos;
|
||||
|
||||
// accumulated quantities;
|
||||
Vector3 force;
|
||||
|
||||
// This is private - only created by the kernel
|
||||
Point(Body* _body = NULL);
|
||||
|
||||
virtual ~Point()
|
||||
{}
|
||||
|
||||
public: // all points from same allocator, size of AttachPoint
|
||||
|
||||
static bool sameBodyAndOffset(const Point& p0, const Point& p1) {
|
||||
return ((p0.body == p1.body) && (p0.localPos == p1.localPos));
|
||||
}
|
||||
|
||||
//////////// called by kernel every step
|
||||
//
|
||||
// Updates World Position, Clears Accumulator
|
||||
|
||||
void step();
|
||||
|
||||
// force accumulation
|
||||
void accumulateForce(const Vector3& _force) {
|
||||
force += _force;
|
||||
}
|
||||
|
||||
// corresponds to "for each Point, accumulate forces to Body"
|
||||
void forceToBody();
|
||||
|
||||
void setLocalPos(const Vector3& _localPos);
|
||||
|
||||
void setWorldPos(const Vector3& _worldPos);
|
||||
|
||||
void setBody(Body* _body) {
|
||||
body = _body;
|
||||
}
|
||||
|
||||
//////////// inquiry
|
||||
Body* getBody() {
|
||||
return body;
|
||||
}
|
||||
|
||||
const Vector3& getWorldPos() {
|
||||
return worldPos;
|
||||
}
|
||||
};
|
||||
|
||||
} // namespace RBX
|
||||
|
||||
@@ -0,0 +1,218 @@
|
||||
/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "V8Kernel/ContactParams.h"
|
||||
#include "V8Kernel/ContactConnector.h"
|
||||
#include "Util/G3DCore.h"
|
||||
#include "rbx/Debug.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// BLOCK BLOCK Types
|
||||
|
||||
class PolyConnector : public ContactConnector
|
||||
{
|
||||
private:
|
||||
int param0;
|
||||
int param1; // for matching
|
||||
|
||||
protected:
|
||||
/*implement*/ virtual GeoPairType getConnectorType() const = 0;
|
||||
|
||||
PolyConnector(
|
||||
Body* b0,
|
||||
Body* b1,
|
||||
const ContactParams& contactParams,
|
||||
int param0,
|
||||
int param1)
|
||||
: ContactConnector(b0, b1, contactParams)
|
||||
, param0(param0)
|
||||
, param1(param1)
|
||||
{}
|
||||
|
||||
public:
|
||||
static bool match(PolyConnector* p0, PolyConnector* p1) {
|
||||
return ( (p0->param0 == p1->param0)
|
||||
&& (p0->param1 == p1->param1)
|
||||
&& (p0->getConnectorType() == p1->getConnectorType()) );
|
||||
}
|
||||
};
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class FaceVertexConnector : public PolyConnector,
|
||||
public Allocator<FaceVertexConnector>
|
||||
{
|
||||
private:
|
||||
Plane facePlane;
|
||||
Vector3 vertexOffset;
|
||||
|
||||
/*override*/ GeoPairType getConnectorType() const {return VERTEX_PLANE_CONNECTOR;}
|
||||
|
||||
public:
|
||||
FaceVertexConnector(
|
||||
Body* b0,
|
||||
Body* b1,
|
||||
const ContactParams& contactParams,
|
||||
const Plane& facePlane,
|
||||
const Vector3& vertexOffset,
|
||||
int planeId,
|
||||
int vertexId)
|
||||
: PolyConnector(b0, b1, contactParams, planeId, vertexId)
|
||||
, facePlane(facePlane)
|
||||
, vertexOffset(vertexOffset)
|
||||
{}
|
||||
|
||||
/*override*/ void updateContactPoint();
|
||||
};
|
||||
|
||||
|
||||
class FaceEdgeConnector : public PolyConnector,
|
||||
public Allocator<FaceEdgeConnector>
|
||||
{
|
||||
private:
|
||||
Plane facePlane;
|
||||
Plane sideFacePlane;
|
||||
Line faceLine;
|
||||
Line edgeLine;
|
||||
|
||||
/*override*/ GeoPairType getConnectorType() const {return EDGE_EDGE_PLANE_CONNECTOR;}
|
||||
|
||||
public:
|
||||
FaceEdgeConnector(
|
||||
Body* b0,
|
||||
Body* b1,
|
||||
const ContactParams& contactParams,
|
||||
const Plane& facePlane,
|
||||
const Plane& sideFacePlane,
|
||||
Line faceLine,
|
||||
Line edgeLine,
|
||||
const int faceId,
|
||||
const int edgeId)
|
||||
: PolyConnector(b0, b1, contactParams, faceId, edgeId)
|
||||
, facePlane(facePlane)
|
||||
, sideFacePlane(sideFacePlane)
|
||||
, faceLine(faceLine)
|
||||
, edgeLine(edgeLine)
|
||||
{}
|
||||
|
||||
/*override*/ void updateContactPoint();
|
||||
};
|
||||
|
||||
class EdgeEdgeConnector : public PolyConnector,
|
||||
public Allocator<EdgeEdgeConnector>
|
||||
{
|
||||
private:
|
||||
Line edgeLine0;
|
||||
Line edgeLine1;
|
||||
|
||||
/*override*/ GeoPairType getConnectorType() const {return EDGE_EDGE_CONNECTOR;}
|
||||
|
||||
public:
|
||||
EdgeEdgeConnector(
|
||||
Body* b0,
|
||||
Body* b1,
|
||||
const ContactParams& contactParams,
|
||||
Line edgeLine0,
|
||||
Line edgeLine1,
|
||||
int edgeId0,
|
||||
int edgeId1 )
|
||||
: PolyConnector(b0, b1, contactParams, edgeId0, edgeId1)
|
||||
, edgeLine0(edgeLine0)
|
||||
, edgeLine1(edgeLine1)
|
||||
{
|
||||
}
|
||||
|
||||
/*override*/ void updateContactPoint();
|
||||
};
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
class BallVertexConnector : public PolyConnector,
|
||||
public Allocator<BallVertexConnector>
|
||||
{
|
||||
private:
|
||||
float radius;
|
||||
Vector3 offset;
|
||||
|
||||
/*override*/ GeoPairType getConnectorType() const {return BALL_VERTEX_CONNECTOR;}
|
||||
|
||||
public:
|
||||
BallVertexConnector(
|
||||
Body* b0,
|
||||
Body* b1,
|
||||
const ContactParams& contactParams,
|
||||
float radius,
|
||||
const Vector3& offset,
|
||||
int vertexId)
|
||||
: PolyConnector(b0, b1, contactParams, 0, vertexId)
|
||||
, radius(radius)
|
||||
, offset(offset)
|
||||
{}
|
||||
|
||||
/*override*/ void updateContactPoint();
|
||||
};
|
||||
|
||||
class BallEdgeConnector : public PolyConnector,
|
||||
public Allocator<BallEdgeConnector>
|
||||
{
|
||||
private:
|
||||
float radius;
|
||||
Vector3 offset;
|
||||
Vector3 normal;
|
||||
|
||||
/*override*/ GeoPairType getConnectorType() const {return BALL_EDGE_CONNECTOR;}
|
||||
|
||||
public:
|
||||
BallEdgeConnector(
|
||||
Body* b0,
|
||||
Body* b1,
|
||||
const ContactParams& contactParams,
|
||||
float radius,
|
||||
const Vector3& offset,
|
||||
const Vector3& normal,
|
||||
int edgeId)
|
||||
: PolyConnector(b0, b1, contactParams, 0, edgeId)
|
||||
, radius(radius)
|
||||
, offset(offset)
|
||||
, normal(normal)
|
||||
{}
|
||||
|
||||
/*override*/ void updateContactPoint();
|
||||
};
|
||||
|
||||
class BallPlaneConnector : public PolyConnector,
|
||||
public Allocator<BallPlaneConnector>
|
||||
{
|
||||
private:
|
||||
float radius;
|
||||
Vector3 offset;
|
||||
Vector3 normal;
|
||||
|
||||
/*override*/ GeoPairType getConnectorType() const {return BALL_PLANE_CONNECTOR;}
|
||||
|
||||
public:
|
||||
BallPlaneConnector(
|
||||
Body* b0,
|
||||
Body* b1,
|
||||
const ContactParams& contactParams,
|
||||
float radius,
|
||||
const Vector3& offset,
|
||||
const Vector3& normal,
|
||||
int faceId)
|
||||
: PolyConnector(b0, b1, contactParams, 0, faceId)
|
||||
, radius(radius)
|
||||
, offset(offset)
|
||||
, normal(normal)
|
||||
{}
|
||||
|
||||
/*override*/ void updateContactPoint();
|
||||
};
|
||||
|
||||
} // namespace RBX
|
||||
@@ -0,0 +1,272 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "Util/G3DCore.h"
|
||||
#include "Util/PV.h"
|
||||
#include "Util/Quaternion.h"
|
||||
#include "Util/Math.h"
|
||||
#include "Util/Memory.h"
|
||||
#include "rbx/threadsafe.h"
|
||||
#include "v8kernel/Constants.h"
|
||||
#include "Fastlog.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
class Body;
|
||||
class SimBody
|
||||
: public Allocator<SimBody>
|
||||
{
|
||||
private:
|
||||
Body* body;
|
||||
float dt;
|
||||
bool dirty;
|
||||
boost::uint64_t uid;
|
||||
|
||||
PV pv;
|
||||
Quaternion qOrientation; // master for simulation
|
||||
Vector3 angMomentum; // master for simulation
|
||||
Vector3 moment;
|
||||
Vector3 momentRecip;
|
||||
Matrix3 momentRecipWorld;
|
||||
float massRecip;
|
||||
float constantForceY;
|
||||
|
||||
// accumulators
|
||||
Vector3 force; // at center of mass, in world coordinates
|
||||
Vector3 torque; // in world coordinates
|
||||
Vector3 impulse; // at center of mass, in world coordinates
|
||||
Vector3 rotationalImpulse; // in world coordinates
|
||||
|
||||
// Cache for impulse solver
|
||||
Vector3 impulseLast;
|
||||
|
||||
int freeFallBodyIndex;
|
||||
int realTimeBodyIndex;
|
||||
int jointBodyIndex;
|
||||
int buoyancyBodyIndex;
|
||||
int contactBodyIndex;
|
||||
|
||||
int numOfConnectors; // how many connectors connect this SimBody (assembly)
|
||||
int numOfHumanoidConnectors;
|
||||
int numOfSecondPassConnectors;
|
||||
int numOfRealTimeConnectors;
|
||||
int numOfJointConnectors;
|
||||
int numOfBuoyancyConnectors;
|
||||
int numOfContactConnectors;
|
||||
|
||||
// Symmetrical state detection
|
||||
bool symmetricContact;
|
||||
bool verticalContact;
|
||||
Vector3 penetrationTorque; // aggregated from contact normals scaled by penetration depth
|
||||
Vector3 penetrationForce; // aggregated from contact normals scaled by penetration depth
|
||||
|
||||
inline void clearForceAccumulators() {
|
||||
force = getWorldGravityForce();
|
||||
torque = Vector3(0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
inline void clearImpulseAccumulators() {
|
||||
impulse = Vector3(0.0, 0.0, 0.0);
|
||||
rotationalImpulse = Vector3(0.0, 0.0, 0.0);
|
||||
}
|
||||
|
||||
|
||||
void update();
|
||||
|
||||
// All debugging stuff;
|
||||
static float maxTorqueXX;
|
||||
static float maxForceXX;
|
||||
static float maxLinearImpulseXX;
|
||||
static float maxRotationalImpulseXX;
|
||||
static float maxDebugTorque();
|
||||
static float maxDebugForce();
|
||||
static float maxDebugLinearImpulse();
|
||||
static float maxDebugRotationalImpulse();
|
||||
|
||||
public:
|
||||
SimBody(Body* body);
|
||||
~SimBody();
|
||||
|
||||
Body* getBody() {return body;}
|
||||
const Body* getBodyConst() const {return body;}
|
||||
void setDt(float _dt) {dt = _dt;}
|
||||
float getDt() const {return dt;}
|
||||
void setUID( boost::uint64_t _uid ) { uid = _uid; }
|
||||
boost::uint64_t getUID() const { return uid; }
|
||||
inline void updateMomentRecipWorld();
|
||||
inline Vector3 computeRotationVelocityFromMomentum();
|
||||
inline Vector3 computeRotationVelocityFromMomentumFast();
|
||||
inline const Matrix3& getInverseInertiaInWorld() const {return momentRecipWorld;}
|
||||
|
||||
void step();
|
||||
void stepVelocity();
|
||||
void stepPosition();
|
||||
void stepFreeFall();
|
||||
|
||||
void applyImpulse(const Vector3& _impulse, const Vector3& worldPos);
|
||||
|
||||
void clearVelocity();
|
||||
void updateAngMomentum();
|
||||
|
||||
void updateFromSolver( const Vector3& newPosition, const Matrix3& newOrientation, const Vector3& newLinearVelocity, const Vector3& newAngularVelocity );
|
||||
|
||||
inline void updateIfDirty() { // called before step. Assumes body cofm is clean
|
||||
if (dirty)
|
||||
update();
|
||||
}
|
||||
|
||||
inline Vector3 getWorldGravityForce() const { return Vector3(0, constantForceY, 0); }
|
||||
inline void clearSymStateAndAccummulator() {
|
||||
symmetricContact = true;
|
||||
verticalContact = true;
|
||||
penetrationTorque = Vector3::zero();
|
||||
penetrationForce = Vector3::zero();
|
||||
}
|
||||
|
||||
inline void makeDirty() {dirty = true;}
|
||||
|
||||
bool getDirty() const {return dirty;}
|
||||
|
||||
inline const PV& getPV() const {return pv;}
|
||||
|
||||
PV getOwnerPV();
|
||||
|
||||
static Vector3 computeTorqueFromOffsetForce(const Vector3& _force, const Vector3& cofm, const Vector3& forceLocationWorld) {
|
||||
Vector3 localPosWorld = forceLocationWorld - cofm;
|
||||
return localPosWorld.cross(_force);
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Parallel physics will accumulate forces from different threads - need a mutex for each
|
||||
//
|
||||
//
|
||||
inline void accumulateForceCofm(const Vector3& _force) {
|
||||
updateIfDirty();
|
||||
force += _force;
|
||||
RBXASSERT_SLOW(force.isFinite());
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(force) < maxDebugForce());
|
||||
}
|
||||
|
||||
inline void accumulateForce(const Vector3& _force, const Vector3& worldPos) {
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(_force) < maxDebugForce());
|
||||
updateIfDirty();
|
||||
force += _force;
|
||||
torque += computeTorqueFromOffsetForce(_force, pv.position.translation, worldPos);
|
||||
RBXASSERT_SLOW(force.isFinite());
|
||||
RBXASSERT_SLOW(torque.isFinite());
|
||||
}
|
||||
|
||||
inline void accumulatePenetrationForce(const Vector3& _force, const Vector3& worldPos) {
|
||||
penetrationForce += _force;
|
||||
penetrationTorque += computeTorqueFromOffsetForce(_force, pv.position.translation, worldPos);
|
||||
}
|
||||
|
||||
inline void accumulateTorque(const Vector3& _torque) {
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(_torque) < maxDebugTorque());
|
||||
updateIfDirty();
|
||||
torque += _torque;
|
||||
RBXASSERT_SLOW(torque.isFinite());
|
||||
}
|
||||
|
||||
inline void accumulateImpulse(const Vector3& _impulse, const Vector3& worldPos) {
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(_impulse) < maxDebugLinearImpulse());
|
||||
updateIfDirty();
|
||||
impulse += _impulse;
|
||||
Vector3 localPosWorld = worldPos - pv.position.translation;
|
||||
rotationalImpulse += localPosWorld.cross(_impulse);
|
||||
RBXASSERT_SLOW(impulse.isFinite());
|
||||
RBXASSERT_SLOW(rotationalImpulse.isFinite());
|
||||
}
|
||||
|
||||
inline void accumulateImpulseAtBranchCofm(const Vector3& _impulse) {
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(_impulse) < maxDebugLinearImpulse());
|
||||
updateIfDirty();
|
||||
impulse += _impulse;
|
||||
RBXASSERT_SLOW(impulse.isFinite());
|
||||
}
|
||||
|
||||
inline void accumulateRotationalImpulse(const Vector3& _rotationalImpulse) {
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(_rotationalImpulse) < maxDebugRotationalImpulse());
|
||||
updateIfDirty();
|
||||
rotationalImpulse += _rotationalImpulse;
|
||||
RBXASSERT_SLOW(rotationalImpulse.isFinite());
|
||||
}
|
||||
// End of parallel section
|
||||
//
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
inline void resetImpulseAccumulators() {
|
||||
updateIfDirty();
|
||||
clearImpulseAccumulators();
|
||||
}
|
||||
|
||||
inline void resetForceAccumulators() {
|
||||
updateIfDirty();
|
||||
clearForceAccumulators();
|
||||
}
|
||||
|
||||
inline const Vector3& getForce() const {return force;}
|
||||
|
||||
inline const Vector3& getTorque() const {return torque;}
|
||||
|
||||
inline const Vector3& getImpulse() const {return impulse;}
|
||||
|
||||
inline const Vector3& getRotationallmpulse() const {return rotationalImpulse;}
|
||||
|
||||
inline const float& getMassRecip() const {return massRecip;}
|
||||
|
||||
inline const Vector3& getImpulseLast() const {return impulseLast;}
|
||||
|
||||
inline bool hasExternalForceOrImpulse() const {return force != getWorldGravityForce() || torque != Vector3::zero() ||
|
||||
impulse != Vector3::zero() || rotationalImpulse != Vector3::zero();}
|
||||
|
||||
inline bool updateSymmetricContactState() {
|
||||
symmetricContact = (penetrationTorque.squaredMagnitude() < Constants::impulseSolverSymStateTorqueBound());
|
||||
verticalContact = ( ( fabs(penetrationForce.x) < Constants::impulseSolverSymStateForceBound() ) &&
|
||||
( fabs(penetrationForce.z) < Constants::impulseSolverSymStateForceBound() ) );
|
||||
|
||||
return symmetricContact;
|
||||
}
|
||||
|
||||
inline bool isSymmetricContact() const {return symmetricContact;}
|
||||
inline bool isVerticalContact() const {return verticalContact;}
|
||||
inline void clearSymmetricContact() {symmetricContact = false;}
|
||||
inline int& getRealTimeBodyIndex() {return realTimeBodyIndex;}
|
||||
inline int& getFreeFallBodyIndex() {return freeFallBodyIndex;}
|
||||
inline int& getJointBodyIndex() {return jointBodyIndex;}
|
||||
inline int& getBuoyancyBodyIndex() {return buoyancyBodyIndex;}
|
||||
inline int& getContactBodyIndex() {return contactBodyIndex;}
|
||||
|
||||
inline bool isFreeFallBody() const {return freeFallBodyIndex >= 0;}
|
||||
inline bool isRealTimeBody() const {return realTimeBodyIndex >= 0;}
|
||||
inline bool isJointBody() const {return jointBodyIndex >= 0; }
|
||||
inline bool isBuoyancyBody() const {return buoyancyBodyIndex >= 0; }
|
||||
inline bool isContactBody() const {return contactBodyIndex >= 0;}
|
||||
inline bool isInKernel() const {return isFreeFallBody() || isRealTimeBody() || isJointBody() || isContactBody() || isBuoyancyBody();}
|
||||
inline bool validateBodyLists() const {return (freeFallBodyIndex >= 0) + (realTimeBodyIndex >= 0) +
|
||||
(jointBodyIndex >= 0) + (contactBodyIndex >= 0) <= 1;}
|
||||
inline const int& getHumanoidConnectorCount() const {return numOfHumanoidConnectors;}
|
||||
inline const int& getSecondPassConnectorCount() const {return numOfSecondPassConnectors;}
|
||||
inline const int& getRealTimeConnectorCount() const {return numOfRealTimeConnectors;}
|
||||
inline const int& getJointConnectorCount() const {return numOfJointConnectors;}
|
||||
inline const int& getBuoyancyConnectorCount() const {return numOfBuoyancyConnectors;}
|
||||
inline const int& getContactConnectorCount() const {return numOfContactConnectors;}
|
||||
inline const int& getConnectorCount() const {return numOfConnectors;}
|
||||
inline void incrementHumanoidConnectorCount() {++numOfHumanoidConnectors; ++numOfConnectors;}
|
||||
inline void decrementHumanoidConnectorCount() {--numOfHumanoidConnectors; --numOfConnectors;}
|
||||
inline void incrementSecondPassConnectorCount() {++numOfSecondPassConnectors; ++numOfConnectors;}
|
||||
inline void decrementSecondPassConnectorCount() {--numOfSecondPassConnectors; --numOfConnectors;}
|
||||
inline void incrementRealTimeConnectorCount() {++numOfRealTimeConnectors; ++numOfConnectors;}
|
||||
inline void decrementRealTimeConnectorCount() {--numOfRealTimeConnectors; --numOfConnectors;}
|
||||
inline void incrementJointConnetorCount() {++numOfJointConnectors; ++numOfConnectors;}
|
||||
inline void decrementJointConnetorCount() {--numOfJointConnectors; --numOfConnectors;}
|
||||
inline void incrementBuoyancyConnectorCount() {++numOfBuoyancyConnectors; ++numOfConnectors;}
|
||||
inline void decrementBuoyancyConnectorCount() {--numOfBuoyancyConnectors; --numOfConnectors;}
|
||||
inline void incrementContactConnectorCount() {++numOfContactConnectors; ++numOfConnectors;}
|
||||
inline void decrementContactConnectorCount() {--numOfContactConnectors; -- numOfConnectors;}
|
||||
};
|
||||
|
||||
} // namespace
|
||||
|
||||
Reference in New Issue
Block a user