mirror of
https://github.com/copyrighttxt/watrbx-game-engine.git
synced 2026-09-06 21:57:47 +00:00
GEEKING
This commit is contained in:
watrabi
@@ -0,0 +1,458 @@
|
||||
/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/Body.h"
|
||||
#include "V8kernel/Kernel.h"
|
||||
#include "V8Kernel/Constants.h"
|
||||
#include "Util/Units.h"
|
||||
|
||||
#include "boost/utility.hpp"
|
||||
#include "rbx/boost.hpp"
|
||||
#include "rbx/atomic.h"
|
||||
|
||||
#include "btBulletCollisionCommon.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
Body* Body::worldBody;
|
||||
|
||||
static rbx::atomic<int> gBodyStateIndex;
|
||||
|
||||
Body::Body()
|
||||
: connectorUseCount(0)
|
||||
, root(NULL)
|
||||
, link(NULL)
|
||||
, leafBodyIndex(-1)
|
||||
, canThrottle(true)
|
||||
, simBody(NULL)
|
||||
, moment(Matrix3::identity())
|
||||
, mass(0.0f)
|
||||
, cofmOffset(Vector3::zero())
|
||||
, stateIndex(getNextStateIndex())
|
||||
, cofm(NULL)
|
||||
, uid( 0 )
|
||||
{
|
||||
root = this;
|
||||
simBody = new SimBody(this);
|
||||
}
|
||||
|
||||
Body::~Body()
|
||||
{
|
||||
RBXASSERT(connectorUseCount == 0);
|
||||
RBXASSERT(!link);
|
||||
RBXASSERT(simBody);
|
||||
RBXASSERT(leafBodyIndex == -1);
|
||||
|
||||
if( cofm )
|
||||
{
|
||||
delete cofm;
|
||||
cofm = NULL;
|
||||
}
|
||||
delete simBody;
|
||||
simBody = NULL;
|
||||
|
||||
RBXASSERT(!cofm);
|
||||
RBXASSERT(root == this);
|
||||
}
|
||||
|
||||
unsigned int Body::getNextStateIndex()
|
||||
{
|
||||
return ++gBodyStateIndex;
|
||||
}
|
||||
|
||||
void Body::advanceStateIndex()
|
||||
{
|
||||
stateIndex = getNextStateIndex();
|
||||
}
|
||||
|
||||
void Body::initStaticData()
|
||||
{
|
||||
worldBody = new Body();
|
||||
}
|
||||
|
||||
Body* Body::getWorldBody()
|
||||
{
|
||||
static boost::once_flag flag = BOOST_ONCE_INIT;
|
||||
boost::call_once(&initStaticData, flag);
|
||||
return worldBody;
|
||||
}
|
||||
|
||||
bool Body::validateParentCofmDirty()
|
||||
{
|
||||
RBXASSERT(cofm);
|
||||
RBXASSERT(cofm->getIsDirty());
|
||||
|
||||
if (getParent()) {
|
||||
getParent()->validateParentCofmDirty();
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
// Goes up the chain, making every Cofm dirty along the way
|
||||
// In addition, forces the root body to become dirty
|
||||
void Body::makeCofmDirty()
|
||||
{
|
||||
if (cofm && cofm->getIsDirty())
|
||||
{
|
||||
RBXASSERT(this->validateParentCofmDirty());
|
||||
RBXASSERT(this->getRootSimBody()->getDirty());
|
||||
}
|
||||
else
|
||||
{
|
||||
if (getParent()) {
|
||||
RBXASSERT(!simBody);
|
||||
getParent()->makeCofmDirty();
|
||||
}
|
||||
else {
|
||||
RBXASSERT(root == this);
|
||||
if (simBody) {
|
||||
simBody->makeDirty();
|
||||
}
|
||||
}
|
||||
|
||||
if (cofm) {
|
||||
cofm->makeDirty();
|
||||
RBXASSERT( (numChildren() > 0) || (cofmOffset != Vector3::zero()) );
|
||||
}
|
||||
else {
|
||||
RBXASSERT(numChildren() == 0);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Body::resetRoot(Body* newRoot)
|
||||
{
|
||||
RBXASSERT(newRoot == calcRoot());
|
||||
root = newRoot;
|
||||
for (int i = 0; i < numChildren(); ++i) {
|
||||
getChild(i)->resetRoot(newRoot);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Body::onParentChanging()
|
||||
{
|
||||
RBXASSERT_VERY_FAST(!getParent() || (!(getParent()->getConstRootSimBody()->isInKernel()))); // confirm not happening to bodies in kernel
|
||||
|
||||
if (link) {
|
||||
link->setBody(NULL); // link must always set parent first, then
|
||||
link = NULL;
|
||||
}
|
||||
|
||||
if (getParent()) {
|
||||
RBXASSERT(root == getParent()->getRoot()); // I keep my pvIndex, as do my children
|
||||
RBXASSERT(!simBody);
|
||||
}
|
||||
else {
|
||||
RBXASSERT(root = this);
|
||||
RBXASSERT(simBody);
|
||||
delete simBody;
|
||||
simBody = NULL;
|
||||
}
|
||||
}
|
||||
|
||||
void Body::onParentChanged(IndexedTree* oldParent)
|
||||
{
|
||||
// confirm not happening to bodies in kernel
|
||||
RBXASSERT_VERY_FAST(!getParent() || (!(getParent()->getConstRootSimBody()->isInKernel())));
|
||||
|
||||
if (getParent()) {
|
||||
;
|
||||
}
|
||||
else {
|
||||
simBody = new SimBody(this);
|
||||
}
|
||||
|
||||
Body* newRoot = calcRoot();
|
||||
newRoot->advanceStateIndex();
|
||||
resetRoot(newRoot);
|
||||
}
|
||||
|
||||
|
||||
void Body::onChildAdding(IndexedTree* child)
|
||||
{
|
||||
makeCofmDirty();
|
||||
}
|
||||
|
||||
void Body::refreshCofm()
|
||||
{
|
||||
bool hasChildren = (numChildren() > 0);
|
||||
bool hasCofmOffset = (cofmOffset != Vector3::zero());
|
||||
|
||||
bool needsCofm = (hasChildren || hasCofmOffset);
|
||||
|
||||
if (needsCofm) {
|
||||
if (!cofm) {
|
||||
cofm = new Cofm(this);
|
||||
RBXASSERT(cofm->getIsDirty());
|
||||
}
|
||||
}
|
||||
else { // doesn't need Cofm
|
||||
if (cofm) {
|
||||
delete cofm;
|
||||
cofm = NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Body::setCofmOffset(const Vector3& _centerOfMassInBody)
|
||||
{
|
||||
|
||||
if (cofmOffset != _centerOfMassInBody) {
|
||||
cofmOffset = _centerOfMassInBody;
|
||||
refreshCofm();
|
||||
makeCofmDirty(); // yes - we need to make dirty down the chain
|
||||
}
|
||||
}
|
||||
|
||||
void Body::updatePV()
|
||||
{
|
||||
RBXASSERT((getParent() != NULL) != (getRoot() == this));
|
||||
|
||||
if (!getParent() || (stateIndex == getRoot()->getStateIndex())) {
|
||||
;
|
||||
}
|
||||
else {
|
||||
getParent()->getPvUnsafe(); // do this first - prevent infinite recursion
|
||||
PV::pvAtLocalCoord(getParent()->getPvUnsafe(), getMeInParent(), pv);
|
||||
|
||||
RBXASSERT(stateIndex != getRoot()->getStateIndex()); // concurrency check
|
||||
stateIndex = getRoot()->getStateIndex();
|
||||
|
||||
RBXASSERT_SLOW(Math::isOrthonormal(pv.position.rotation));
|
||||
RBXASSERT_SLOW(!Math::isNanInfDenormVector3(pv.position.translation)); // - asserting all the time - John
|
||||
}
|
||||
}
|
||||
|
||||
void Body::onChildAdded(IndexedTree* child)
|
||||
{
|
||||
if (!cofm) {
|
||||
RBXASSERT(numChildren() == 1);
|
||||
refreshCofm();
|
||||
RBXASSERT(cofm);
|
||||
} // no need to make dirty - adding anyways
|
||||
}
|
||||
|
||||
void Body::onChildRemoved(IndexedTree* child)
|
||||
{
|
||||
RBXASSERT(cofm);
|
||||
if (numChildren() == 0) {
|
||||
refreshCofm();
|
||||
}
|
||||
makeCofmDirty();
|
||||
}
|
||||
|
||||
// Note - need to reset the root state index, because this is what everyone uses to determine if they're in synch
|
||||
//
|
||||
//
|
||||
void Body::setMeInParent(const CoordinateFrame& _meInParent)
|
||||
{
|
||||
RBXASSERT_FISHING(Math::longestVector3Component(_meInParent.translation) < 1e6);
|
||||
|
||||
RBXASSERT_FISHING(Math::isOrthonormal(_meInParent.rotation));
|
||||
|
||||
if (link) {
|
||||
RBXASSERT(0);
|
||||
link->setBody(NULL);
|
||||
link = NULL;
|
||||
}
|
||||
|
||||
if (getParent()) {
|
||||
meInParent = _meInParent;
|
||||
makeCofmDirty();
|
||||
makeStateDirty();
|
||||
}
|
||||
else {
|
||||
RBXASSERT(0);
|
||||
}
|
||||
}
|
||||
|
||||
void Body::setMeInParent(Link* _link)
|
||||
{
|
||||
RBXASSERT(_link);
|
||||
|
||||
if (link && (link != _link)) {
|
||||
link->setBody(NULL);
|
||||
}
|
||||
|
||||
if (getParent()) {
|
||||
link = _link;
|
||||
link->setBody(this);
|
||||
makeCofmDirty();
|
||||
makeStateDirty();
|
||||
}
|
||||
else {
|
||||
RBXASSERT(0);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void Body::setPv(const PV& _pv, const BodyPvSetter& bpv)
|
||||
{
|
||||
if (getParent())
|
||||
{
|
||||
RBXASSERT(0); // bad news here - this object has a parent, so setting it's position should only be done
|
||||
// by changing the parent's position, or meInParent
|
||||
}
|
||||
else
|
||||
{
|
||||
pv = _pv;
|
||||
if (simBody) {
|
||||
simBody->makeDirty();
|
||||
}
|
||||
advanceStateIndex(); // I'm the root
|
||||
}
|
||||
|
||||
//RBXASSERT(Math::longestVector3Component(pv.position.translation) < 1e6);
|
||||
RBXASSERT_FISHING(Math::isOrthonormal(pv.position.rotation));
|
||||
RBXASSERT_SLOW(!Math::isNanInfDenormVector3(pv.position.translation));
|
||||
// RBXASSERT(!Math::isNanInfDenormMatrix3(pv.position.rotation)); -- This was asserting all the time. I had to comment out - John
|
||||
RBXASSERT_SLOW(!Math::isNanInfDenormVector3(pv.velocity.linear));
|
||||
RBXASSERT_SLOW(!Math::isNanInfDenormVector3(pv.velocity.rotational));
|
||||
}
|
||||
|
||||
void Body::setCoordinateFrame(const CoordinateFrame& worldCoord, const BodyPvSetter& bpv)
|
||||
{
|
||||
setPv( PV(worldCoord, getVelocity()), bpv );
|
||||
}
|
||||
|
||||
void Body::setVelocity(const Velocity& worldVelocity, const BodyPvSetter& bpv)
|
||||
{
|
||||
if (!getParent()) {
|
||||
pv.velocity = worldVelocity;
|
||||
advanceStateIndex();
|
||||
if (simBody) {
|
||||
simBody->makeDirty();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Body::setCanThrottle(bool value, const BodyPvSetter& bpv)
|
||||
{
|
||||
canThrottle = value;
|
||||
}
|
||||
|
||||
|
||||
void Body::setMass(float _mass)
|
||||
{
|
||||
if (mass != _mass) {
|
||||
makeCofmDirty();
|
||||
mass = _mass;
|
||||
}
|
||||
}
|
||||
|
||||
void Body::setMoment(const Matrix3& _momentInBody)
|
||||
{
|
||||
if (moment != _momentInBody) {
|
||||
makeCofmDirty();
|
||||
moment = _momentInBody;
|
||||
}
|
||||
}
|
||||
|
||||
Matrix3 Body::getIBodyAtPoint(const Vector3& point)
|
||||
{
|
||||
return Math::getIBodyAtPoint( point,
|
||||
getIBody(),
|
||||
getMass() );
|
||||
}
|
||||
|
||||
Matrix3 Body::getIWorldAtPoint(const Vector3& point)
|
||||
{
|
||||
return Math::getIWorldAtPoint( getPos(),
|
||||
point,
|
||||
getIWorld(),
|
||||
getMass() );
|
||||
}
|
||||
|
||||
Matrix3 Body::getBranchIWorldAtPoint(const Vector3& point)
|
||||
{
|
||||
return Math::getIWorldAtPoint( getBranchCofmPos(),
|
||||
point,
|
||||
getBranchIWorld(),
|
||||
getBranchMass() );
|
||||
}
|
||||
|
||||
const Vector3& Body::getBranchCofmOffset()
|
||||
{
|
||||
RBXASSERT(simBody);
|
||||
RBXASSERT((numChildren() > 0) || !getCofm() || getCofm()->getCofmInBody() == cofmOffset);
|
||||
|
||||
return getCofm() ? getCofm()->getCofmInBody() : cofmOffset;
|
||||
}
|
||||
|
||||
Vector3 Body::getBranchCofmPos()
|
||||
{
|
||||
return cofm ? getCoordinateFrame().pointToWorldSpace(cofm->getCofmInBody())
|
||||
: getPos();
|
||||
}
|
||||
|
||||
CoordinateFrame Body::getBranchCofmCoordinateFrame()
|
||||
{
|
||||
return CoordinateFrame( getCoordinateFrame().rotation,
|
||||
getBranchCofmPos() );
|
||||
}
|
||||
|
||||
|
||||
float Body::potentialEnergy()
|
||||
{
|
||||
PV tempPv = getPvUnsafe();
|
||||
|
||||
float rbxGravity = Units::kmsAccelerationToRbx(Constants::getKmsGravity());
|
||||
|
||||
return -rbxGravity * tempPv.position.translation.y * mass;
|
||||
}
|
||||
|
||||
float Body::kineticEnergy()
|
||||
{
|
||||
PV tempPv = getPvUnsafe();
|
||||
|
||||
// E = 0.5 (Iw) dot w
|
||||
Vector3 Iw = getIWorld() * tempPv.velocity.rotational;
|
||||
float out = 0.5f * Iw.dot(tempPv.velocity.rotational) + 0.5f * tempPv.velocity.linear.dot(tempPv.velocity.linear) * mass;
|
||||
return out;
|
||||
}
|
||||
|
||||
void Body::setUID( boost::uint64_t _uid )
|
||||
{
|
||||
uid = _uid;
|
||||
if( simBody )
|
||||
{
|
||||
simBody->setUID( _uid );
|
||||
}
|
||||
}
|
||||
|
||||
void Body::updateBulletCollisionObject(btCollisionObject* object)
|
||||
{
|
||||
btTransform transform = getPvUnsafe().position.transformFromCFrame();
|
||||
|
||||
if (btCollisionShape* shape = object->getCollisionShape())
|
||||
{
|
||||
btMatrix3x3& basis = transform.getBasis();
|
||||
|
||||
if (shape->getShapeType() == SPHERE_SHAPE_PROXYTYPE)
|
||||
{
|
||||
basis.setIdentity();
|
||||
}
|
||||
else if (shape->getShapeType() == CYLINDER_SHAPE_PROXYTYPE)
|
||||
{
|
||||
// column 0 = axis X - cylinder is rotationally invariant around this axis
|
||||
btVector3 axisX = basis.getColumn(0);
|
||||
|
||||
// find a basis with axisX as the axis
|
||||
// unfortunately any basis like that has a discontinuity so sometimes the rotation will flip
|
||||
// we'll try to pick the axis so that the discontinuity is when the cylinder mostly stands upright
|
||||
// in this case it's less apparent since the cylinder is unlikely to spin fast while standing on its top
|
||||
btVector3 axisP = fabsf(axisX.getY()) < 0.9f ? btVector3(0, 1, 0) : btVector3(1, 0, 0);
|
||||
btVector3 axisZ = axisX.cross(axisP).normalized();
|
||||
btVector3 axisY = axisZ.cross(axisX).normalized();
|
||||
|
||||
basis.setValue(axisX.getX(), axisY.getX(), axisZ.getX(), axisX.getY(), axisY.getY(), axisZ.getY(), axisX.getZ(), axisY.getZ(), axisZ.getZ());
|
||||
}
|
||||
}
|
||||
|
||||
object->setWorldTransform(transform);
|
||||
}
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,247 @@
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/BulletShapeConnectors.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
#include "Util/Math.h"
|
||||
|
||||
#include "BulletCollision/NarrowphaseCollision/btPersistentManifold.h"
|
||||
#include "btBulletCollisionCommon.h"
|
||||
|
||||
namespace RBX {
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/*
|
||||
Position == position in world coordinates of deepest penetration point.
|
||||
Length == (negative) value - amount of penetration
|
||||
Normal == points "away" from the b0 object, into the b1 object
|
||||
*/
|
||||
BulletShapeConnector::~BulletShapeConnector()
|
||||
{
|
||||
if (bulletAlgo)
|
||||
{
|
||||
btManifoldArray manArray;
|
||||
bulletAlgo->getAllContactManifolds(manArray);
|
||||
for (int i = 0; i < manArray[bulletManifoldIndex]->getNumContacts(); i++)
|
||||
{
|
||||
if (manArray[bulletManifoldIndex]->getContactPoint(i).m_userPersistentData == this)
|
||||
{
|
||||
manArray[bulletManifoldIndex]->getContactPoint(i).m_userPersistentData = NULL;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
bool BulletShapeConnector::validObjectCFrames()
|
||||
{
|
||||
//Check if the Vector3's are Nans
|
||||
return (!(Math::isNanInfVector3(geoPair.body0->getCoordinateFrame().translation) || (Math::isNanInfVector3(geoPair.body1->getCoordinateFrame().translation))));
|
||||
}
|
||||
|
||||
void BulletShapeConnector::updateBulletCollisionObjects()
|
||||
{
|
||||
geoPair.body0->updateBulletCollisionObject(bulletCollisionObject0);
|
||||
geoPair.body1->updateBulletCollisionObject(bulletCollisionObject1);
|
||||
}
|
||||
|
||||
void BulletShapeConnector::refreshIndividualPoint(bool swapped, Vector3 pt0InWorld, Vector3 pt1InWorld, btManifoldArray& manArray)
|
||||
{
|
||||
if ((manArray[bulletManifoldIndex]->getNumContacts() > bulletPointCacheIndex))
|
||||
{
|
||||
bool pointInvalid = false;
|
||||
double contactThreshold = (double) manArray[bulletManifoldIndex]->getContactBreakingThreshold();
|
||||
{
|
||||
btManifoldPoint &connectorPoint = manArray[bulletManifoldIndex]->getContactPoint(bulletPointCacheIndex);
|
||||
updatePointWithTransform(swapped, connectorPoint);
|
||||
pointInvalid = isPointInvalid( connectorPoint, contactThreshold );
|
||||
}
|
||||
|
||||
if (pointInvalid)
|
||||
{
|
||||
recalculateValidPoints(manArray, pt0InWorld, pt1InWorld);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void BulletShapeConnector::updatePointWithTransform(bool swapped, btManifoldPoint& manifoldPoint)
|
||||
{
|
||||
btTransform tr0;
|
||||
btTransform tr1;
|
||||
if (swapped)
|
||||
{
|
||||
tr1 = bulletCollisionObject0->getWorldTransform();
|
||||
tr0 = bulletCollisionObject1->getWorldTransform();
|
||||
}
|
||||
else
|
||||
{
|
||||
tr0 = bulletCollisionObject0->getWorldTransform();
|
||||
tr1 = bulletCollisionObject1->getWorldTransform();
|
||||
}
|
||||
manifoldPoint.m_positionWorldOnA = tr0( manifoldPoint.m_localPointA );
|
||||
manifoldPoint.m_positionWorldOnB = tr1( manifoldPoint.m_localPointB );
|
||||
manifoldPoint.m_distance1 = (manifoldPoint.m_positionWorldOnA - manifoldPoint.m_positionWorldOnB).dot(manifoldPoint.m_normalWorldOnB);
|
||||
manifoldPoint.m_lifeTime++;
|
||||
}
|
||||
|
||||
bool BulletShapeConnector::isPointInvalid(btManifoldPoint& manifoldPoint, double validThreshold)
|
||||
{
|
||||
if (!(manifoldPoint.m_distance1 <= validThreshold))
|
||||
{
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
btVector3 projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
|
||||
btVector3 projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;
|
||||
btScalar distance2d = projectedDifference.dot(projectedDifference);
|
||||
if (distance2d > gContactThresholdOrthogonalFactor * gContactThresholdOrthogonalFactor * validThreshold * validThreshold)
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
void BulletShapeConnector::findValidContactAfterNarrowphase()
|
||||
{
|
||||
updateConnectorPointFromManifold(false);
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
void BulletShapeConnector::updateConnectorPointFromManifold(bool refreshContacts)
|
||||
{
|
||||
btManifoldArray manifoldArray;
|
||||
bulletAlgo->getAllContactManifolds(manifoldArray);
|
||||
|
||||
RBXASSERT(manifoldArray.size() > bulletManifoldIndex);
|
||||
Vector3 pt0InWorld, pt1InWorld;
|
||||
bool hasValidPoints = false;
|
||||
bool swapped = !(manifoldArray[bulletManifoldIndex]->getBody0() == bulletCollisionObject0);
|
||||
if (manifoldArray[bulletManifoldIndex]->getNumContacts() > bulletPointCacheIndex &&
|
||||
(manifoldArray[bulletManifoldIndex]->getContactPoint(bulletPointCacheIndex).m_userPersistentData == this))
|
||||
{
|
||||
if (refreshContacts)
|
||||
{
|
||||
refreshIndividualPoint(swapped, pt0InWorld, pt1InWorld, manifoldArray);
|
||||
}
|
||||
|
||||
if (swapped)
|
||||
{
|
||||
hasValidPoints = foundValidContactPointFromBulletManifold(manifoldArray[bulletManifoldIndex], pt1InWorld, pt0InWorld);
|
||||
}
|
||||
else
|
||||
{
|
||||
hasValidPoints = foundValidContactPointFromBulletManifold(manifoldArray[bulletManifoldIndex], pt0InWorld, pt1InWorld);
|
||||
}
|
||||
}
|
||||
|
||||
if (hasValidPoints)
|
||||
{
|
||||
const btManifoldPoint& conPoint = manifoldArray[bulletManifoldIndex]->getContactPoint(bulletPointCacheIndex);
|
||||
|
||||
contactPoint.position = pt0InWorld;
|
||||
contactPoint.normal = pt0InWorld - pt1InWorld;
|
||||
contactPoint.length = conPoint.getDistance() < 0.0 ? -contactPoint.normal.unitize() : contactPoint.normal.unitize();
|
||||
}
|
||||
else
|
||||
{
|
||||
contactPoint.length = 1.0;
|
||||
}
|
||||
}
|
||||
|
||||
bool BulletShapeConnector::recalculateValidPoints(btManifoldArray& btManArray, Vector3& pt0InWorld, Vector3& pt1InWorld)
|
||||
{
|
||||
btManArray.clear();
|
||||
|
||||
btCollisionObjectWrapper obj0Wrap(0, bulletCollisionObject0->getCollisionShape(), bulletCollisionObject0 , bulletCollisionObject0 ->getWorldTransform(), -1, -1);
|
||||
btCollisionObjectWrapper obj1Wrap(0, bulletCollisionObject1->getCollisionShape(), bulletCollisionObject1, bulletCollisionObject1->getWorldTransform(), -1, -1);
|
||||
btManifoldResult contactPointResult(&obj0Wrap, &obj1Wrap);
|
||||
btDispatcherInfo disInfo;
|
||||
bulletAlgo->processCollision(&obj0Wrap, &obj1Wrap, disInfo, &contactPointResult);
|
||||
|
||||
bulletAlgo->getAllContactManifolds(btManArray);
|
||||
if (bulletCollisionObject0->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE ||
|
||||
bulletCollisionObject1->getCollisionShape()->getShapeType() == GIMPACT_SHAPE_PROXYTYPE)
|
||||
{
|
||||
//GImpact collision algorithm should only yield 1 Manifold
|
||||
RBXASSERT(btManArray.size() <= 1);
|
||||
if (btManArray.size())
|
||||
{
|
||||
btManArray[0]->refreshContactPoints(bulletCollisionObject0->getWorldTransform(), bulletCollisionObject1->getWorldTransform());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// processCollision may swap points if we use btCompoundShape, we have to detect this.
|
||||
if (btManArray[bulletManifoldIndex]->getBody0() == bulletCollisionObject0)
|
||||
{
|
||||
if (foundValidContactPointFromBulletManifold(btManArray[bulletManifoldIndex], pt0InWorld, pt1InWorld))
|
||||
return true;
|
||||
}
|
||||
else
|
||||
{
|
||||
if (foundValidContactPointFromBulletManifold(btManArray[bulletManifoldIndex], pt1InWorld, pt0InWorld))
|
||||
return true;
|
||||
}
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
bool BulletShapeConnector::foundValidContactPointFromBulletManifold(btPersistentManifold* man, Vector3& p0World, Vector3& p1World)
|
||||
{
|
||||
realignConnectorsToBulletContacts();
|
||||
|
||||
// After the bullet refresh, this connector may no longer be valid, so we must check and skip if necessary
|
||||
// To be valid, the bulletPointCacheIndex must within range and the m_userPersistentData must match "this" connector
|
||||
if ((bulletPointCacheIndex < man->getNumContacts() && man->getContactPoint(bulletPointCacheIndex).m_userPersistentData == this))
|
||||
{
|
||||
btManifoldPoint conPoint = man->getContactPoint(bulletPointCacheIndex);
|
||||
p1World = Vector3(conPoint.getPositionWorldOnB().x(), conPoint.getPositionWorldOnB().y(), conPoint.getPositionWorldOnB().z());
|
||||
p0World = p1World + conPoint.getDistance() * Vector3(conPoint.m_normalWorldOnB.x(), conPoint.m_normalWorldOnB.y(), conPoint.m_normalWorldOnB.z());
|
||||
|
||||
return true;
|
||||
}
|
||||
else // skip this contact by making its length > 0 so it is ignored by the kernel force solve
|
||||
return false;
|
||||
}
|
||||
|
||||
void BulletShapeConnector::realignConnectorsToBulletContacts()
|
||||
{
|
||||
btManifoldArray manifoldArray;
|
||||
bulletAlgo->getAllContactManifolds(manifoldArray);
|
||||
for (int i = 0; i < manifoldArray[bulletManifoldIndex]->getNumContacts(); i++)
|
||||
{
|
||||
BulletShapeConnector* conn = rbx_static_cast<BulletShapeConnector*>(manifoldArray[bulletManifoldIndex]->getContactPoint(i).m_userPersistentData);
|
||||
if (conn)
|
||||
conn->setBulletManifoldPointIndex(i);
|
||||
}
|
||||
}
|
||||
|
||||
void BulletShapeConnector::updateContactPoint()
|
||||
{
|
||||
//Prevent Nans from being calculated in Physics
|
||||
if (!validObjectCFrames())
|
||||
return;
|
||||
|
||||
updateBulletCollisionObjects();
|
||||
updateConnectorPointFromManifold();
|
||||
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
void BulletShapeCellConnector::updateBulletCollisionObjects()
|
||||
{
|
||||
geoPair.body1->updateBulletCollisionObject(bulletCollisionObject1);
|
||||
}
|
||||
|
||||
void BulletShapeCellConnector::updateContactPoint()
|
||||
{
|
||||
updateBulletCollisionObjects();
|
||||
updateConnectorPointFromManifold();
|
||||
|
||||
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,39 @@
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "v8Kernel/BuoyancyConnector.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
|
||||
namespace RBX
|
||||
{
|
||||
const Vector3 BuoyancyConnector::getWorldPosition()
|
||||
{
|
||||
const CoordinateFrame& cf(getBody(body1)->getPvSafe().position);
|
||||
return cf.pointToWorldSpace(position);
|
||||
}
|
||||
|
||||
void BuoyancyConnector::computeForce( bool throttling )
|
||||
{
|
||||
getBody(body1)->accumulateForce(force, getWorldPosition());
|
||||
getBody(body1)->accumulateTorque(torque);
|
||||
}
|
||||
|
||||
BuoyancyConnector::BuoyancyConnector(Body* b0, Body* b1, const Vector3& pos) :
|
||||
ContactConnector(b0, b1, ContactParams()),
|
||||
position(pos),
|
||||
force(Vector3::zero()),
|
||||
torque(Vector3::zero()),
|
||||
floatDistance(0.0f),
|
||||
sinkDistance(0.0f),
|
||||
submergeRatio(0.0f)
|
||||
{
|
||||
}
|
||||
|
||||
void BuoyancyConnector::updateContactPoint()
|
||||
{
|
||||
// This is used for debug rendering only
|
||||
const CoordinateFrame& cf(getBody(body1) ->getPvUnsafe().position);
|
||||
contactPoint.position = cf.pointToWorldSpace(position);
|
||||
contactPoint.normal = force.direction();
|
||||
contactPoint.length = -force.magnitude() / 3000.0f;
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,60 @@
|
||||
/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/Cofm.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
|
||||
#include "V8Kernel/Constants.h"
|
||||
#include "Util/Units.h"
|
||||
#include "Util/Math.h"
|
||||
#include "rbx/Debug.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
Cofm::Cofm(Body* body)
|
||||
: body(body)
|
||||
, dirty(true)
|
||||
{}
|
||||
|
||||
void Cofm::updateIfDirty()
|
||||
{
|
||||
if (dirty) {
|
||||
mass = body->getMass();
|
||||
|
||||
if (body->numChildren() == 0) {
|
||||
cofmInBody = body->getCofmOffset();
|
||||
moment = body->getIBodyAtPoint(cofmInBody);
|
||||
|
||||
RBXASSERT(Math::fuzzyEq(moment,
|
||||
Math::momentToObjectSpace(body->getIWorldAtPoint(body->getCoordinateFrame().pointToWorldSpace(cofmInBody)),
|
||||
body->getCoordinateFrame().rotation),
|
||||
moment.l1Norm() * 0.01));
|
||||
}
|
||||
else {
|
||||
Vector3 bodyCofmOffset = body->getCofmOffset();
|
||||
Vector3 bodyCofmInWorld = body->getCoordinateFrame().pointToWorldSpace(bodyCofmOffset); // toWorldSpace(bodyCofmOffset);
|
||||
|
||||
Vector3 cofmWorld = bodyCofmInWorld * body->getMass(); // Needs to transform and add offset.
|
||||
for (int i = 0; i < body->numChildren(); ++i) {
|
||||
Body* b = body->getChild(i);
|
||||
mass += b->getBranchMass();
|
||||
cofmWorld += b->getBranchCofmPos() * b->getBranchMass();
|
||||
}
|
||||
cofmWorld = cofmWorld / mass;
|
||||
cofmInBody = body->getCoordinateFrame().pointToObjectSpace(cofmWorld);
|
||||
|
||||
Matrix3 iWorldSum = body->getIWorldAtPoint(cofmWorld);
|
||||
for (int i = 0; i < body->numChildren(); ++i) {
|
||||
iWorldSum = iWorldSum + body->getChild(i)->getBranchIWorldAtPoint(cofmWorld);
|
||||
}
|
||||
moment = Math::momentToObjectSpace(iWorldSum, body->getCoordinateFrame().rotation);
|
||||
}
|
||||
|
||||
RBXASSERT(dirty); // concurrency issues?
|
||||
dirty = false;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,212 @@
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/Connector.h"
|
||||
#include "V8Kernel/Point.h"
|
||||
#include "V8Kernel/Constants.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
#include "Util/Math.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
|
||||
bool Connector::computeCanThrottle()
|
||||
{
|
||||
return (getBody(body0)->getCanThrottle() && getBody(body1)->getCanThrottle());
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
void PointToPointBreakConnector::forceToPoints(const G3D::Vector3& force)
|
||||
{
|
||||
point0->accumulateForce(-force);
|
||||
point1->accumulateForce(force);
|
||||
RBXASSERT_SLOW(force.magnitude() < Math::inf());
|
||||
}
|
||||
|
||||
Body* PointToPointBreakConnector::getBody(BodyIndex id)
|
||||
{
|
||||
return (id == body0) ? point0->getBody() : point1->getBody();
|
||||
}
|
||||
|
||||
|
||||
/////////////////////////////////////////////////
|
||||
|
||||
RotateConnector::RotateConnector(
|
||||
Body* _b0,
|
||||
Body* _b1,
|
||||
const CoordinateFrame& _j0,
|
||||
const CoordinateFrame& _j1,
|
||||
float _baseAngle,
|
||||
float kValue,
|
||||
float armLength)
|
||||
: b0(_b0)
|
||||
, b1(_b1)
|
||||
, j0(_j0)
|
||||
, j1(_j1)
|
||||
, baseRotation(_baseAngle)
|
||||
, k(kValue * armLength * armLength)
|
||||
, currentAngle(0.0f)
|
||||
, desiredAngle(0.0f)
|
||||
, increment(0.0f)
|
||||
, zeroVelocity(false)
|
||||
{
|
||||
reset();
|
||||
}
|
||||
|
||||
void RotateConnector::reset() // occurs after networking read;
|
||||
{
|
||||
Vector3 tempNormal;
|
||||
desiredAngle = computeNormalRotationFromBase(tempNormal); // reset to match current rotation
|
||||
}
|
||||
|
||||
Body* RotateConnector::getBody(BodyIndex id)
|
||||
{
|
||||
return (id == body0) ? b0 : b1;
|
||||
}
|
||||
|
||||
float RotateConnector::computeNormalRotationFromBase(Vector3& normal)
|
||||
{
|
||||
float angle = computeJointAngle( b0->getCoordinateFrame(),
|
||||
b1->getCoordinateFrame(),
|
||||
j0,
|
||||
j1,
|
||||
normal);
|
||||
|
||||
float answer = angle - baseRotation;
|
||||
RBXASSERT_FISHING(fabs(answer) <= Math::twoPif());
|
||||
return answer;
|
||||
}
|
||||
|
||||
|
||||
float RotateConnector::computeNormalRotationFromBaseFast(Vector3& normal)
|
||||
{
|
||||
float angle = computeJointAngle( b0->getCoordinateFrameFast(),
|
||||
b1->getCoordinateFrameFast(),
|
||||
j0,
|
||||
j1,
|
||||
normal);
|
||||
|
||||
float answer = angle - baseRotation;
|
||||
RBXASSERT(fabs(answer) <= Math::twoPif());
|
||||
return answer;
|
||||
}
|
||||
|
||||
float RotateConnector::computeJointAngle(const CoordinateFrame& b0,
|
||||
const CoordinateFrame& b1,
|
||||
const CoordinateFrame& j0,
|
||||
const CoordinateFrame& j1,
|
||||
Vector3& normal)
|
||||
{
|
||||
/*
|
||||
CoordinateFrame j0World = b0 * j0;
|
||||
CoordinateFrame j1World = b1 * j1;
|
||||
CoordinateFrame j1Inj0 = j0World.toObjectSpace(j1World);
|
||||
|
||||
normal = j0World.rotation.getColumn(2);
|
||||
float rot = Math::zAxisAngle(j1Inj0);
|
||||
*/
|
||||
|
||||
Matrix3 j0World = b0.rotation * j0.rotation;
|
||||
Matrix3 j1World = b1.rotation * j1.rotation;
|
||||
Matrix3 j1Inj0 = j0World.transpose() * j1World;
|
||||
normal = j0World.column(2);
|
||||
float rot = Math::zAxisAngle(j1Inj0);
|
||||
return rot;
|
||||
}
|
||||
|
||||
|
||||
void RotateConnector::setRotationalGoal(float newGoal)
|
||||
{
|
||||
float normalizedGoal = static_cast<float>(Math::radWrap(newGoal));
|
||||
|
||||
float deltaRotation = Math::deltaRotationClose(normalizedGoal, desiredAngle);
|
||||
|
||||
increment = deltaRotation / Constants::kernelStepsPerWorldStep();
|
||||
}
|
||||
|
||||
|
||||
void RotateConnector::setVelocityGoal(float velocity)
|
||||
{
|
||||
// velocity unit: radian per long ui step
|
||||
increment = velocity * Constants::longUiStepsPerSec() * Constants::kernelDt();
|
||||
if (velocity == 0.0)
|
||||
{
|
||||
zeroVelocity = true;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
void RotateConnector::stepGoals()
|
||||
{
|
||||
if (zeroVelocity)
|
||||
{
|
||||
zeroVelocity = false;
|
||||
desiredAngle = Math::averageRotationClose(currentAngle, desiredAngle);
|
||||
}
|
||||
desiredAngle += increment;
|
||||
}
|
||||
|
||||
|
||||
void RotateConnector::computeForce(bool throttling)
|
||||
{
|
||||
Vector3 normal;
|
||||
currentAngle = computeNormalRotationFromBaseFast(normal); // between -2pi and 2pi
|
||||
|
||||
stepGoals(); // between -pi and pi
|
||||
|
||||
float deltaRotation = Math::deltaRotationClose(desiredAngle, currentAngle); // between -pi and pi;
|
||||
|
||||
RBXASSERT(fabs(deltaRotation) <= Math::pif());
|
||||
|
||||
float torqueVal = k * deltaRotation;
|
||||
Vector3 torque = normal * torqueVal;
|
||||
|
||||
b0->accumulateTorque(-torque);
|
||||
b1->accumulateTorque(torque);
|
||||
}
|
||||
|
||||
|
||||
/////////////////////////////////////////////////
|
||||
|
||||
float PointToPointBreakConnector::potentialEnergy()
|
||||
{
|
||||
Vector3 delta = point1->getWorldPos() - point0->getWorldPos();
|
||||
float length = delta.length();
|
||||
return length * length * k * 0.5f;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void PointToPointBreakConnector::computeForce(bool throttling)
|
||||
{
|
||||
Vector3 force = -k * (point1->getWorldPos() - point0->getWorldPos());
|
||||
float mag = Math::taxiCabMagnitude(force);
|
||||
forceToPoints(force);
|
||||
|
||||
broken = (mag > breakForce);
|
||||
}
|
||||
|
||||
|
||||
/////////////////////////////////////////////////
|
||||
|
||||
// normal direction is "out" from the body surface
|
||||
// delta = P1 - P0, where P0 is the body with the normal direction
|
||||
//
|
||||
|
||||
void NormalBreakConnector::computeForce(bool throttling)
|
||||
{
|
||||
Vector3 normal = Math::getWorldNormal( normalIdBody0,
|
||||
point0->getBody()->getCoordinateFrameFast().rotation);
|
||||
|
||||
Vector3 force = -k * (point1->getWorldPos() - point0->getWorldPos());
|
||||
float magApart = -normal.dot(force);
|
||||
forceToPoints(force);
|
||||
|
||||
broken = (magApart > breakForce);
|
||||
}
|
||||
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,492 @@
|
||||
/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/Constants.h"
|
||||
#include "Util/Math.h"
|
||||
#include "Util/Units.h"
|
||||
#include "rbx/Debug.h"
|
||||
#include <vector>
|
||||
#include <algorithm>
|
||||
|
||||
namespace RBX {
|
||||
|
||||
//static int kernelStepsPerWorldStepTest = 80;
|
||||
//static float originalHzFactorTest = 19.0f;
|
||||
|
||||
Constants::Constants()
|
||||
{}
|
||||
|
||||
int Constants::worldStepsPerLongUiStep() {return 8;}
|
||||
|
||||
int Constants::worldStepsPerUiStep() {return worldStepsPerLongUiStep() / 2;}
|
||||
|
||||
int Constants::kernelStepsPerWorldStep() {return 19;}
|
||||
int Constants::freeFallStepsPerWorldStep(){return 1;}
|
||||
//const int Constants::kernelStepsPerWorldStep() {return kernelStepsPerWorldStepTest;}
|
||||
int Constants::worldStepsPerSec() {return uiStepsPerSec() * worldStepsPerUiStep();}
|
||||
int Constants::kernelStepsPerSec() {return worldStepsPerSec() * kernelStepsPerWorldStep();}
|
||||
int Constants::kernelStepsPerUiStep() {return kernelStepsPerWorldStep() * worldStepsPerUiStep();}
|
||||
int Constants::freeFallStepsPerSec() {return worldStepsPerSec() * freeFallStepsPerWorldStep();}
|
||||
int Constants::impulseSolverMaxIterations() {return 60;}
|
||||
float Constants::impulseSolverAccuracy() {return 0.1;} // in unit of relative velocity
|
||||
int Constants::impulseSolverAccuracyScalar() {return 400;} // how many parts to dial down accuracy for one level
|
||||
float Constants::impulseSolverSymStateTorqueBound() {return 0.05f;}
|
||||
float Constants::impulseSolverSymStateForceBound() {return 0.001f;}
|
||||
float Constants::uiDt() {return 1.0f / uiStepsPerSec();}
|
||||
float Constants::longUiStepDt() {return 1.0f / longUiStepsPerSec();}
|
||||
float Constants::worldDt() {return 1.0f / worldStepsPerSec();}
|
||||
float Constants::kernelDt() {return 1.0f / kernelStepsPerSec();}
|
||||
float Constants::freeFallDt() {return 1.0f / freeFallStepsPerSec();}
|
||||
|
||||
// original joint K calcs were done and created the default jointK for 1x1x1 block of 960000 at 4560hz
|
||||
float originalHz() {return 30.0f * 8.0f * 19.0f;}
|
||||
//const float originalHz() {return 30.0f * 8.0f * originalHzFactorTest;}
|
||||
float Constants::unitJointK() {return 960000.0f / (originalHz() * originalHz());}
|
||||
|
||||
float Constants::getElasticMultiplier(float elasticity)
|
||||
{
|
||||
if (elasticity < 0.05f) return 0.28f;
|
||||
else if (elasticity < 0.26f) return 0.42f;
|
||||
else if (elasticity < 0.51f) return 0.57f;
|
||||
else if (elasticity < 0.76f) return 0.8f;
|
||||
else return 1.0f;
|
||||
}
|
||||
|
||||
|
||||
|
||||
// These are constants for LEGO, taken from actual measurements
|
||||
// See:
|
||||
// Evolution of Complexity in Real-World Domains
|
||||
// Pablo Funes
|
||||
// Brandeis University
|
||||
//
|
||||
// The original TORQUE chart is here:
|
||||
/*
|
||||
Joint size () Approximate torque capacity ( )
|
||||
knobs N-m
|
||||
1 12.7
|
||||
2 61.5
|
||||
3 109.8
|
||||
4 192.7
|
||||
5 345.0
|
||||
6 424.0
|
||||
*/
|
||||
// This chart was converted from Torques to forces
|
||||
// See "Joint Breakage.xlr"
|
||||
// then factored by the length of the moment arm
|
||||
// this if for a LEGO with 3 grid units high in the y direction
|
||||
//
|
||||
// This is the edge force for a one-unit wide beam.
|
||||
// Point forces for a one-unit wide beam will be 1/2 of this value
|
||||
// Point forces for a two-unit wide beam will equal this value
|
||||
const float Constants::MAX_LEGO_JOINT_FORCES_THEORY[] = { 0.0f, // kg*m/s^2
|
||||
1.5875f,
|
||||
3.84375f,
|
||||
4.5750f,
|
||||
6.0210f,
|
||||
8.6250f,
|
||||
8.8330f };
|
||||
|
||||
// These are the values I got from actual measurements
|
||||
const float Constants::MAX_LEGO_JOINT_FORCES_MEASURED[] = { 0.0f, // kg*m/s^2
|
||||
1.0980f,
|
||||
2.1340f,
|
||||
2.4270f,
|
||||
3.1910f,
|
||||
4.5710f,
|
||||
4.6810f };
|
||||
|
||||
// To convert to other unit systems for same general behavior
|
||||
//
|
||||
// Consider a 1x3x4 brick in Lego World - overlapping by one grid
|
||||
// Mass: 1.6 gram or 0.0016 kg
|
||||
// Fg = m*Ag = 15.696 mm/s^2 - same force is applied on the opposing "up" direction
|
||||
// so - only need to scale by the mass per grid unit....
|
||||
|
||||
float Constants::LEGO_GRID_MASS()
|
||||
{
|
||||
// a 1x3x1 unit weighs about 0.4 grams;
|
||||
// convert to kg (*0.001)
|
||||
// divide by 3
|
||||
return (float) (0.4 * 0.001 * 0.33333333); // kg, divide by 3 for y layers
|
||||
}
|
||||
|
||||
|
||||
float Constants::LEGO_JOINT_K() {
|
||||
return 35000.0; // kg/s^2
|
||||
}
|
||||
|
||||
|
||||
float Constants::getKmsMaxJointForce(float grid1, float grid2) // kg*mm/s^2
|
||||
{
|
||||
RBXASSERT(std::abs(grid1*10.0f - Math::iRound(grid1*10.0f)) < 0.01f); // Can't enter with values too far off grid
|
||||
RBXASSERT(std::abs(grid2*10.0f - Math::iRound(grid2*10.0f)) < 0.01f); // indicates a bad snap
|
||||
// for now - all snapes should be on units of 0.2
|
||||
int grid1int = std::max(1, Math::iRound(grid1));
|
||||
int grid2int = std::max(1, Math::iRound(grid2));
|
||||
|
||||
int overlap = std::max(grid1int, grid2int); // note - should be an integer, but handle non-int as well
|
||||
int width = std::min(grid1int, grid2int);
|
||||
|
||||
float oneWideEdgeForce;
|
||||
|
||||
if (overlap < JOINT_FORCE_DATA) {
|
||||
oneWideEdgeForce = MAX_LEGO_JOINT_FORCES_MEASURED[overlap];
|
||||
}
|
||||
else { // for now, just keep increasing force proportional to ratio...
|
||||
float ratio = static_cast<float>(overlap);
|
||||
ratio /= JOINT_FORCE_DATA;
|
||||
oneWideEdgeForce = MAX_LEGO_JOINT_FORCES_MEASURED[JOINT_FORCE_DATA - 1] * ratio;
|
||||
}
|
||||
|
||||
float oneWidePointForce = oneWideEdgeForce * 0.5f; // because force values are for the edge force, not a point force
|
||||
// of which there are two
|
||||
float maxPointForce = oneWidePointForce * width; // scale for width;
|
||||
|
||||
return maxPointForce * (1.0f / LEGO_GRID_MASS());
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
|
||||
_____________________________________________________
|
||||
JOINT Optimizer Results
|
||||
|
||||
Part Type x y z Optimized Recommended RATIO
|
||||
BALL 1 1 1 0.23 1.00 0.23
|
||||
BALL 2 2 2 1.49 2.64 0.56
|
||||
BALL 3 3 3 4.43 6.75 0.66
|
||||
BALL 4 4 4 11.50 16.00 0.72
|
||||
BALL 6 6 6 37.13 54.00 0.69
|
||||
BALL 10 10 10 171.88 250.00 0.69
|
||||
BALL 15 15 15 606.45 843.75 0.72
|
||||
BALL 20 20 20 1437.50 2000.00 0.72
|
||||
BALL 40 40 40 12000.00 16000.00 0.75
|
||||
BALL 80 80 80 104000.00 128000.00 0.81
|
||||
BLOCK 1 1 1 0.91 1.00 0.91
|
||||
BLOCK 1 1 2 1.61 1.66 0.97
|
||||
BLOCK 1 1 3 2.00 2.00 1.00
|
||||
BLOCK 1 1 4 2.13 3.33 0.64
|
||||
BLOCK 1 1 6 2.92 4.80 0.61
|
||||
BLOCK 1 1 10 4.63 8.00 0.58
|
||||
BLOCK 1 1 15 6.00 12.00 0.50
|
||||
BLOCK 1 1 20 8.50 16.00 0.53
|
||||
BLOCK 1 1 40 17.00 32.00 0.53
|
||||
BLOCK 1 1 80 32.50 64.00 0.51
|
||||
BLOCK 1 2 2 3.50 4.00 0.88
|
||||
BLOCK 1 2 3 4.16 5.33 0.78
|
||||
BLOCK 1 2 4 4.79 6.66 0.72
|
||||
BLOCK 1 2 6 6.98 7.98 0.88
|
||||
BLOCK 1 2 10 9.56 13.30 0.72
|
||||
BLOCK 1 2 15 14.34 19.95 0.72
|
||||
BLOCK 1 2 20 17.46 26.60 0.66
|
||||
BLOCK 1 2 40 31.59 53.20 0.59
|
||||
BLOCK 1 2 80 63.17 106.40 0.59
|
||||
BLOCK 1 3 3 6.07 4.98 1.22
|
||||
BLOCK 1 3 4 7.47 6.64 1.13
|
||||
BLOCK 1 3 6 10.27 9.96 1.03
|
||||
BLOCK 1 3 10 14.01 16.60 0.84
|
||||
BLOCK 1 3 15 21.01 24.90 0.84
|
||||
BLOCK 1 3 20 26.98 33.20 0.81
|
||||
BLOCK 1 3 40 49.80 66.40 0.75
|
||||
BLOCK 1 3 80 95.45 132.80 0.72
|
||||
BLOCK 1 4 4 9.65 7.92 1.22
|
||||
BLOCK 1 4 6 13.36 11.88 1.13
|
||||
BLOCK 1 4 10 19.18 19.80 0.97
|
||||
BLOCK 1 4 15 27.84 29.70 0.94
|
||||
BLOCK 1 4 20 35.89 39.60 0.91
|
||||
BLOCK 1 4 40 64.35 79.20 0.81
|
||||
BLOCK 1 4 80 123.75 158.40 0.78
|
||||
BLOCK 1 6 6 20.79 15.84 1.31
|
||||
BLOCK 1 6 10 33.83 26.40 1.28
|
||||
BLOCK 1 6 15 42.08 39.60 1.06
|
||||
BLOCK 1 6 20 51.15 52.80 0.97
|
||||
BLOCK 1 6 40 95.70 105.60 0.91
|
||||
BLOCK 1 6 80 198.00 211.20 0.94
|
||||
BLOCK 1 10 10 50.74 39.60 1.28
|
||||
BLOCK 1 10 15 70.54 59.40 1.19
|
||||
BLOCK 1 10 20 89.10 79.20 1.13
|
||||
BLOCK 1 10 40 163.35 158.40 1.03
|
||||
BLOCK 1 10 80 326.70 316.80 1.03
|
||||
BLOCK 1 15 15 110.45 84.15 1.31
|
||||
BLOCK 1 15 20 129.73 112.20 1.16
|
||||
BLOCK 1 15 40 280.50 224.40 1.25
|
||||
BLOCK 1 15 80 532.95 448.80 1.19
|
||||
BLOCK 1 20 20 186.04 145.20 1.28
|
||||
BLOCK 1 20 40 363.00 290.40 1.25
|
||||
BLOCK 1 20 80 635.25 580.80 1.09
|
||||
BLOCK 1 40 40 710.33 554.40 1.28
|
||||
BLOCK 1 40 80 1316.70 1108.80 1.19
|
||||
BLOCK 1 80 80 2706.00 2164.80 1.25
|
||||
BLOCK 2 2 2 7.34 2.64 2.78
|
||||
BLOCK 2 2 3 9.90 3.96 2.50
|
||||
BLOCK 2 2 4 11.22 5.28 2.13
|
||||
BLOCK 2 2 6 13.36 7.92 1.69
|
||||
BLOCK 2 2 10 21.45 13.20 1.63
|
||||
BLOCK 2 2 15 29.70 19.80 1.50
|
||||
BLOCK 2 2 20 37.95 26.40 1.44
|
||||
BLOCK 2 2 40 64.35 52.80 1.22
|
||||
BLOCK 2 2 80 122.10 105.60 1.16
|
||||
BLOCK 2 3 3 15.96 5.94 2.69
|
||||
BLOCK 2 3 4 19.06 7.92 2.41
|
||||
BLOCK 2 3 6 22.46 11.88 1.89
|
||||
BLOCK 2 3 10 32.17 19.80 1.63
|
||||
BLOCK 2 3 15 42.69 29.70 1.44
|
||||
BLOCK 2 3 20 51.98 39.60 1.31
|
||||
BLOCK 2 3 40 99.00 79.20 1.25
|
||||
BLOCK 2 3 80 183.15 158.40 1.16
|
||||
BLOCK 2 4 4 25.41 10.56 2.41
|
||||
BLOCK 2 4 6 32.17 15.84 2.03
|
||||
BLOCK 2 4 10 42.90 26.40 1.63
|
||||
BLOCK 2 4 15 56.92 39.60 1.44
|
||||
BLOCK 2 4 20 69.30 52.80 1.31
|
||||
BLOCK 2 4 40 128.70 105.60 1.22
|
||||
BLOCK 2 4 80 244.20 211.20 1.16
|
||||
BLOCK 2 6 6 44.92 23.76 1.89
|
||||
BLOCK 2 6 10 74.25 39.60 1.88
|
||||
BLOCK 2 6 15 100.24 59.40 1.69
|
||||
BLOCK 2 6 20 118.80 79.20 1.50
|
||||
BLOCK 2 6 40 198.00 158.40 1.25
|
||||
BLOCK 2 6 80 386.10 316.80 1.22
|
||||
BLOCK 2 10 10 115.50 66.00 1.75
|
||||
BLOCK 2 10 15 142.31 99.00 1.44
|
||||
BLOCK 2 10 20 214.50 132.00 1.63
|
||||
BLOCK 2 10 40 346.50 264.00 1.31
|
||||
BLOCK 2 10 80 693.00 528.00 1.31
|
||||
BLOCK 2 15 15 232.03 148.50 1.56
|
||||
BLOCK 2 15 20 284.63 198.00 1.44
|
||||
BLOCK 2 15 40 519.75 396.00 1.31
|
||||
BLOCK 2 15 80 990.00 792.00 1.25
|
||||
BLOCK 2 20 20 379.50 264.00 1.44
|
||||
BLOCK 2 20 40 726.00 528.00 1.38
|
||||
BLOCK 2 20 80 1353.00 1056.00 1.28
|
||||
BLOCK 2 40 40 1452.00 1056.00 1.38
|
||||
BLOCK 2 40 80 2904.00 2112.00 1.38
|
||||
BLOCK 2 80 80 5808.00 4224.00 1.38
|
||||
BLOCK 3 3 3 23.84 6.75 3.53
|
||||
BLOCK 3 3 4 30.09 9.00 3.34
|
||||
BLOCK 3 3 6 37.55 13.50 2.78
|
||||
BLOCK 3 3 10 49.92 22.50 2.22
|
||||
BLOCK 3 3 15 71.72 33.75 2.13
|
||||
BLOCK 3 3 20 85.08 45.00 1.89
|
||||
BLOCK 3 3 40 151.88 90.00 1.69
|
||||
BLOCK 3 3 80 303.75 180.00 1.69
|
||||
BLOCK 3 4 4 43.50 12.00 3.63
|
||||
BLOCK 3 4 6 55.13 18.00 3.06
|
||||
BLOCK 3 4 10 75.00 30.00 2.50
|
||||
BLOCK 3 4 15 95.63 45.00 2.13
|
||||
BLOCK 3 4 20 113.44 60.00 1.89
|
||||
BLOCK 3 4 40 210.00 120.00 1.75
|
||||
BLOCK 3 4 80 405.00 240.00 1.69
|
||||
BLOCK 3 6 6 85.22 27.00 3.16
|
||||
BLOCK 3 6 10 112.50 45.00 2.50
|
||||
BLOCK 3 6 15 156.09 67.50 2.31
|
||||
BLOCK 3 6 20 174.38 90.00 1.94
|
||||
BLOCK 3 6 40 337.50 180.00 1.88
|
||||
BLOCK 3 6 80 540.00 360.00 1.50
|
||||
BLOCK 3 10 10 187.50 75.00 2.50
|
||||
BLOCK 3 10 15 260.16 112.50 2.31
|
||||
BLOCK 3 10 20 290.63 150.00 1.94
|
||||
BLOCK 3 10 40 562.50 300.00 1.88
|
||||
BLOCK 3 10 80 1050.00 600.00 1.75
|
||||
BLOCK 3 15 15 358.59 168.75 2.13
|
||||
BLOCK 3 15 20 478.13 225.00 2.13
|
||||
BLOCK 3 15 40 871.88 450.00 1.94
|
||||
BLOCK 3 15 80 1518.75 900.00 1.69
|
||||
BLOCK 3 20 20 637.50 300.00 2.13
|
||||
BLOCK 3 20 40 1087.50 600.00 1.81
|
||||
BLOCK 3 20 80 2100.00 1200.00 1.75
|
||||
BLOCK 3 40 40 2175.00 1200.00 1.81
|
||||
BLOCK 3 40 80 3900.00 2400.00 1.63
|
||||
BLOCK 3 80 80 9000.00 4800.00 1.88
|
||||
BLOCK 4 4 4 59.75 16.00 3.73
|
||||
BLOCK 4 4 6 87.00 24.00 3.63
|
||||
BLOCK 4 4 10 107.50 40.00 2.69
|
||||
BLOCK 4 4 15 116.25 60.00 1.94
|
||||
BLOCK 4 4 20 145.00 80.00 1.81
|
||||
BLOCK 4 4 40 300.00 160.00 1.88
|
||||
BLOCK 4 4 80 560.00 320.00 1.75
|
||||
BLOCK 4 6 6 127.13 36.00 3.53
|
||||
BLOCK 4 6 10 155.63 60.00 2.59
|
||||
BLOCK 4 6 15 208.13 90.00 2.31
|
||||
BLOCK 4 6 20 243.75 120.00 2.03
|
||||
BLOCK 4 6 40 435.00 240.00 1.81
|
||||
BLOCK 4 6 80 750.00 480.00 1.56
|
||||
BLOCK 4 10 10 278.13 100.00 2.78
|
||||
BLOCK 4 10 15 360.94 150.00 2.41
|
||||
BLOCK 4 10 20 425.00 200.00 2.13
|
||||
BLOCK 4 10 40 750.00 400.00 1.88
|
||||
BLOCK 4 10 80 1450.00 800.00 1.81
|
||||
BLOCK 4 15 15 562.50 225.00 2.50
|
||||
BLOCK 4 15 20 637.50 300.00 2.13
|
||||
BLOCK 4 15 40 1134.38 600.00 1.89
|
||||
BLOCK 4 15 80 1950.00 1200.00 1.63
|
||||
BLOCK 4 20 20 925.00 400.00 2.31
|
||||
BLOCK 4 20 40 1625.00 800.00 2.03
|
||||
BLOCK 4 20 80 2700.00 1600.00 1.69
|
||||
BLOCK 4 40 40 3250.00 1600.00 2.03
|
||||
BLOCK 4 40 80 5800.00 3200.00 1.81
|
||||
BLOCK 4 80 80 12100.00 6400.00 1.89
|
||||
BLOCK 6 6 6 180.56 54.00 3.34
|
||||
BLOCK 6 6 10 233.44 90.00 2.59
|
||||
BLOCK 6 6 15 312.19 135.00 2.31
|
||||
BLOCK 6 6 20 365.63 180.00 2.03
|
||||
BLOCK 6 6 40 652.50 360.00 1.81
|
||||
BLOCK 6 6 80 1260.00 720.00 1.75
|
||||
BLOCK 6 10 10 487.50 150.00 3.25
|
||||
BLOCK 6 10 15 583.59 225.00 2.59
|
||||
BLOCK 6 10 20 750.00 300.00 2.50
|
||||
BLOCK 6 10 40 1275.00 600.00 2.13
|
||||
BLOCK 6 10 80 2175.00 1200.00 1.81
|
||||
BLOCK 6 15 15 875.39 337.50 2.59
|
||||
BLOCK 6 15 20 1082.81 450.00 2.41
|
||||
BLOCK 6 15 40 1743.75 900.00 1.94
|
||||
BLOCK 6 15 80 3150.00 1800.00 1.75
|
||||
BLOCK 6 20 20 1556.25 600.00 2.59
|
||||
BLOCK 6 20 40 2325.00 1200.00 1.94
|
||||
BLOCK 6 20 80 4200.00 2400.00 1.75
|
||||
BLOCK 6 40 40 4875.00 2400.00 2.03
|
||||
BLOCK 6 40 80 9000.00 4800.00 1.88
|
||||
BLOCK 6 80 80 18600.00 9600.00 1.94
|
||||
BLOCK 10 10 10 789.06 250.00 3.16
|
||||
BLOCK 10 10 15 1078.13 375.00 2.88
|
||||
BLOCK 10 10 20 1250.00 500.00 2.50
|
||||
BLOCK 10 10 40 2031.25 1000.00 2.03
|
||||
BLOCK 10 10 80 3625.00 2000.00 1.81
|
||||
BLOCK 10 15 15 1880.86 562.50 3.34
|
||||
BLOCK 10 15 20 2296.88 750.00 3.06
|
||||
BLOCK 10 15 40 3468.75 1500.00 2.31
|
||||
BLOCK 10 15 80 5625.00 3000.00 1.88
|
||||
BLOCK 10 20 20 2875.00 1000.00 2.88
|
||||
BLOCK 10 20 40 4625.00 2000.00 2.31
|
||||
BLOCK 10 20 80 7750.00 4000.00 1.94
|
||||
BLOCK 10 40 40 9250.00 4000.00 2.31
|
||||
BLOCK 10 40 80 15500.00 8000.00 1.94
|
||||
BLOCK 10 80 80 31000.00 16000.00 1.94
|
||||
BLOCK 15 15 15 2742.19 843.75 3.25
|
||||
BLOCK 15 15 20 3339.84 1125.00 2.97
|
||||
BLOCK 15 15 40 5414.06 2250.00 2.41
|
||||
BLOCK 15 15 80 8718.75 4500.00 1.94
|
||||
BLOCK 15 20 20 5015.62 1500.00 3.34
|
||||
BLOCK 15 20 40 7218.75 3000.00 2.41
|
||||
BLOCK 15 20 80 11625.00 6000.00 1.94
|
||||
BLOCK 15 40 40 17250.00 6000.00 2.88
|
||||
BLOCK 15 40 80 26625.00 12000.00 2.22
|
||||
BLOCK 15 80 80 53250.00 24000.00 2.22
|
||||
BLOCK 20 20 20 6500.00 2000.00 3.25
|
||||
BLOCK 20 20 40 9250.00 4000.00 2.31
|
||||
BLOCK 20 20 80 16250.00 8000.00 2.03
|
||||
BLOCK 20 40 40 23000.00 8000.00 2.88
|
||||
BLOCK 20 40 80 37000.00 16000.00 2.31
|
||||
BLOCK 20 80 80 80000.00 32000.00 2.50
|
||||
BLOCK 40 40 40 55000.00 16000.00 3.44
|
||||
BLOCK 40 40 80 74000.00 32000.00 2.31
|
||||
BLOCK 40 80 80 184000.00 64000.00 2.88
|
||||
BLOCK 80 80 80 440000.00 128000.00 3.44
|
||||
*/
|
||||
|
||||
|
||||
// New version from actual data - June 30, 2005
|
||||
float Constants::getJointKMultiplier(const Vector3& clippedSortedSize, bool ball)
|
||||
{
|
||||
RBXASSERT(clippedSortedSize.y >= clippedSortedSize.x);
|
||||
RBXASSERT(clippedSortedSize.z >= clippedSortedSize.y);
|
||||
RBXASSERT(clippedSortedSize.max(Vector3(1,1,1)) == clippedSortedSize);
|
||||
|
||||
Vector3int16 size(clippedSortedSize);
|
||||
|
||||
if (ball)
|
||||
{
|
||||
RBXASSERT(size.x >= 1);
|
||||
|
||||
switch (size.x)
|
||||
{
|
||||
case 1: return 0.23f;
|
||||
case 2: return 1.49f;
|
||||
case 3: return 4.43f;
|
||||
case 4: return 11.50f;
|
||||
default: return size.x * size.x * size.x * 0.175f;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
switch (size.x)
|
||||
{
|
||||
case 1: //////////// 1 thick table
|
||||
switch (size.y)
|
||||
{
|
||||
case 1: // 1*1*n
|
||||
switch (size.z)
|
||||
{
|
||||
case 1: return 0.91f;
|
||||
case 2: return 1.61f;
|
||||
case 3: return 2.0f;
|
||||
case 4: return 2.13f;
|
||||
default: return size.z * 0.4f;
|
||||
}
|
||||
|
||||
case 2: // 1*2*n
|
||||
switch (size.z)
|
||||
{
|
||||
case 2: return 3.5f;
|
||||
case 3: return 4.16f;
|
||||
case 4: return 4.79f;
|
||||
default: return (size.z < 15.0f) ? size.z * 0.9f : size.z * 0.75f;
|
||||
}
|
||||
|
||||
case 3: // 1*3*n
|
||||
return (size.z < 7.0f) ? size.z * 1.66f : size.z * 1.18f;
|
||||
|
||||
case 4: // 1*4*n
|
||||
return (size.z < 7.0f) ? size.z * 2.26f : size.z * 1.53f;
|
||||
|
||||
default:
|
||||
return size.z * (size.y * 0.3f + 0.66f);
|
||||
}
|
||||
|
||||
case 2: //////////// 2 thick table
|
||||
switch (size.y)
|
||||
{
|
||||
case 2: // 2*2*n
|
||||
switch (size.z)
|
||||
{
|
||||
case 2: return 7.34f;
|
||||
case 3: return 9.90f;
|
||||
case 4: return 11.22f;
|
||||
default: return (size.z < 15.0f) ? size.z * 1.9f : size.z * 1.5f;
|
||||
}
|
||||
|
||||
case 3: // 2*3*n
|
||||
switch (size.z)
|
||||
{
|
||||
case 3: return 15.0f;
|
||||
case 4: return 19.0f;
|
||||
default: return (size.z < 15.0f) ? size.z * 2.0f : size.z * 1.5f;
|
||||
}
|
||||
|
||||
default:
|
||||
return size.z * (size.y * 0.66f);
|
||||
}
|
||||
|
||||
default: ////////////// at least 3 thick
|
||||
return size.x * size.y * size.z * 0.25f;
|
||||
}
|
||||
}
|
||||
|
||||
float Constants::getJointK(const Vector3& size, bool ball)
|
||||
{
|
||||
Vector3 sortedSize = Math::sortVector3(size);
|
||||
|
||||
Vector3 clippedSize = sortedSize.max(Vector3(1.0f,1.0f,1.0f));
|
||||
|
||||
float sizeMultiplier = getJointKMultiplier(clippedSize, ball);
|
||||
|
||||
if (sortedSize[0] < 1.0) {
|
||||
sizeMultiplier *= sortedSize[0];
|
||||
}
|
||||
|
||||
return sizeMultiplier * kernelStepsPerSec() * kernelStepsPerSec() * unitJointK();
|
||||
}
|
||||
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,478 @@
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/ContactConnector.h"
|
||||
#include "V8Kernel/Constants.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
#include "Util/Math.h"
|
||||
|
||||
FASTFLAGVARIABLE( BallBlockNarrowphaseFixEnabled, false )
|
||||
|
||||
namespace RBX {
|
||||
|
||||
int ContactConnector::inContactHit = 0;
|
||||
int ContactConnector::outOfContactHit = 0;
|
||||
|
||||
float ContactConnector::percentActive()
|
||||
{
|
||||
int denom = inContactHit + outOfContactHit;
|
||||
float answer = (denom == 0)
|
||||
? -1
|
||||
: (float)(inContactHit) / (float)(denom);
|
||||
|
||||
inContactHit = 0;
|
||||
outOfContactHit = 0; // even looking at this function resets it :-)
|
||||
return answer;
|
||||
}
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
|
||||
bool ContactConnector::canThrottle() const
|
||||
{
|
||||
return (geoPair.body0->getCanThrottle() && geoPair.body1->getCanThrottle());
|
||||
}
|
||||
|
||||
float ContactConnector::computeRelativeVelocity(const PairParams ¶ms, Vector3* deltaVnormal, Vector3* perpVel)
|
||||
{
|
||||
// deltaV is body1's linear velocity relative to body0
|
||||
Vector3 velocity0 = geoPair.body0->getPvUnsafe().linearVelocityAtPoint(params.position);
|
||||
Vector3 velocity1 = geoPair.body1->getPvUnsafe().linearVelocityAtPoint(params.position);
|
||||
Vector3 deltaV = velocity1 - velocity0;
|
||||
|
||||
// params.normal points from p0 to p1
|
||||
float normalVel = params.normal.dot(deltaV);
|
||||
if (deltaVnormal)
|
||||
*deltaVnormal = params.normal * normalVel;
|
||||
if (perpVel)
|
||||
*perpVel = deltaV - params.normal * normalVel;
|
||||
|
||||
return normalVel;
|
||||
}
|
||||
|
||||
float ContactConnector::computeRelativeVelocity()
|
||||
{
|
||||
updateContactPoint(); // TBD: check if this is absolutely necessary
|
||||
// return positive velocity when body1 is approaching bpdy0
|
||||
return -computeRelativeVelocity(contactPoint, NULL, NULL);
|
||||
}
|
||||
|
||||
// Reorder the SimBody(s) so that simBody0 is always in kernel and adjust contact point data accordingly
|
||||
bool ContactConnector::getReordedSimBody(SimBody*& simBody0, SimBody*& simBody1, Body*& bodyNotInKernel, PairParams& params)
|
||||
{
|
||||
simBody0 = geoPair.body0->getRootSimBody();
|
||||
simBody1 = geoPair.body1->getRootSimBody();
|
||||
bodyNotInKernel = geoPair.body0;
|
||||
|
||||
RBXASSERT(simBody0 && simBody1);
|
||||
|
||||
if (!simBody0->isInKernel())
|
||||
{
|
||||
// Make sure simBody0 is always in kernel
|
||||
if (!simBody1->isInKernel())
|
||||
return false;
|
||||
simBody0 = simBody1;
|
||||
simBody1 = NULL;
|
||||
params.normal *= -1.0f;
|
||||
} else if (!simBody1->isInKernel())
|
||||
{
|
||||
simBody1 = NULL;
|
||||
bodyNotInKernel = geoPair.body1;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool ContactConnector::getReordedSimBody(SimBody*& simBody0, SimBody*& simBody1, PairParams& params)
|
||||
{
|
||||
Body* dummy = NULL;
|
||||
return getReordedSimBody(simBody0, simBody1, dummy, params);
|
||||
}
|
||||
|
||||
// Compute the relative velocities between the two bodies
|
||||
bool ContactConnector::getSimBodyAndContactVelocity(SimBody*& simBody0, SimBody*& simBody1, PairParams& params,
|
||||
float& normalVel, Vector3& perpVel)
|
||||
{
|
||||
|
||||
|
||||
Body* bodyNotInKernel = NULL;
|
||||
if (!getReordedSimBody(simBody0, simBody1, bodyNotInKernel, params))
|
||||
{
|
||||
outOfContactHit++;
|
||||
return false;
|
||||
}
|
||||
|
||||
RBXASSERT(simBody0 || simBody1);
|
||||
inContactHit++;
|
||||
|
||||
Vector3 deltaV = -simBody0->getPV().linearVelocityAtPoint(params.position);
|
||||
if (simBody1)
|
||||
deltaV += simBody1->getPV().linearVelocityAtPoint(params.position);
|
||||
else
|
||||
deltaV += bodyNotInKernel->getPvUnsafe().linearVelocityAtPoint(params.position);
|
||||
|
||||
normalVel = params.normal.dot(deltaV); // params.normal points from simBody0 to simBody1
|
||||
perpVel = deltaV - params.normal * normalVel;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void ContactConnector::computeForce(bool throttling)
|
||||
{
|
||||
RBXASSERT(!throttling || !canThrottle());
|
||||
|
||||
updateContactPoint();
|
||||
const PairParams& params = getContactPoint();
|
||||
|
||||
if (params.length < 0.0)
|
||||
{
|
||||
inContactHit++;
|
||||
|
||||
Vector3 deltaVnormal, perpVel;
|
||||
float normalVel = computeRelativeVelocity(params, &deltaVnormal, &perpVel);
|
||||
|
||||
frictionOffset += perpVel * Constants::kernelDt();
|
||||
|
||||
float frictionSpringK = contactParams.kSpring * 0.2f;
|
||||
float currentForce = frictionOffset.length() * frictionSpringK;
|
||||
float maxForce = forceMagLast * contactParams.kFriction;
|
||||
if ((currentForce > maxForce) && (currentForce > 1e-8)) { // dynamic friction here
|
||||
frictionOffset *= (maxForce / currentForce); // lose energy here!
|
||||
}
|
||||
// could do every 8th frame
|
||||
frictionOffset -= params.normal * (params.normal.dot(frictionOffset)); // remove no-planar component
|
||||
|
||||
threshold = (threshold != 0.0f)
|
||||
? (-overlapGoal() + ((threshold + overlapGoal()) * 0.999f))
|
||||
: (firstApproach == 0.0f) ? 0.0f : params.length;
|
||||
|
||||
firstApproach = (firstApproach != 0.0f)
|
||||
? (-overlapGoal() + ((firstApproach + overlapGoal()) * 0.999f))
|
||||
: params.length;
|
||||
|
||||
// normal force stuff
|
||||
float kApplied = (normalVel < 0) ? contactParams.kSpring : contactParams.kNeg; // different contact on rebound
|
||||
forceMagLast = kApplied * (firstApproach- params.length); // should be zero first time through
|
||||
forceMagLast = (params.length <= threshold) ? forceMagLast : 0.0f;
|
||||
|
||||
Vector3 force = (forceMagLast * params.normal) - (frictionOffset * frictionSpringK);
|
||||
|
||||
// force to bodies
|
||||
geoPair.body0->accumulateForce(-force, params.position);
|
||||
geoPair.body1->accumulateForce(force, params.position);
|
||||
RBXASSERT(fabs(force.x) < Math::inf());
|
||||
}
|
||||
else {
|
||||
outOfContactHit++;
|
||||
reset();
|
||||
return;
|
||||
}
|
||||
}
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// IMPULSE SOLVER
|
||||
//
|
||||
// Invoked from the kernel step in the following sequence
|
||||
//
|
||||
// stepVelocity()
|
||||
// for (i == 0; i < nIteration; ++i)
|
||||
// {
|
||||
// for each contact connector
|
||||
// {
|
||||
// computeImpulse()
|
||||
// stepImpulse()
|
||||
// }
|
||||
// }
|
||||
// stepPosition()
|
||||
//
|
||||
/////////////////////////////////////////////////////////////////////////////////////////
|
||||
static float REST_THRESHOLD = 1.0f;
|
||||
static float PENETRATION_BIAS_WEIGHT = 0.1f;
|
||||
static float FRICTION_GAIN = 1.2f;
|
||||
static float SYM_CONTACT_MAX_TANGENT_VEL = 1.0f;
|
||||
static float SYM_CONTACT_MAX_NORMAL_VEL = 60.0f;
|
||||
|
||||
bool ContactConnector::computeImpulse(float& residualVelocity)
|
||||
{
|
||||
RBXASSERT(geoPair.body0->getRootSimBody()->isInKernel() || geoPair.body1->getRootSimBody()->isInKernel());
|
||||
|
||||
// params.normal points from contact point of body0 to outside body0
|
||||
// params.length points from contact point of body0 to outside body0
|
||||
|
||||
PairParams params = getContactPoint();
|
||||
|
||||
if (params.length >= 0.0f) // body0 and body1 are not inter-penetrating
|
||||
{
|
||||
outOfContactHit++;
|
||||
reset();
|
||||
return false;
|
||||
}
|
||||
|
||||
SimBody *simBody0, *simBody1;
|
||||
float normalVel;
|
||||
Vector3 perpVel;
|
||||
|
||||
if (!getSimBodyAndContactVelocity(simBody0, simBody1, params, normalVel, perpVel))
|
||||
return false;
|
||||
|
||||
bool symetricContact = true;
|
||||
bool verticalSymetricContact = true;
|
||||
float normalVelSize = fabs(normalVel);
|
||||
// only update sym state in the first iteration
|
||||
bool velocityOutOfRange = (normalVelSize > SYM_CONTACT_MAX_NORMAL_VEL || perpVel.squaredMagnitude() > SYM_CONTACT_MAX_TANGENT_VEL);
|
||||
|
||||
if (simBody0->isSymmetricContact())
|
||||
{
|
||||
if (velocityOutOfRange)
|
||||
{
|
||||
simBody0->clearSymmetricContact();
|
||||
symetricContact = verticalSymetricContact = false;
|
||||
} else if (!simBody0->isVerticalContact())
|
||||
verticalSymetricContact = false;
|
||||
} else
|
||||
symetricContact = verticalSymetricContact = false;
|
||||
|
||||
if (simBody1)
|
||||
{
|
||||
if (simBody1->isSymmetricContact())
|
||||
{
|
||||
if (velocityOutOfRange)
|
||||
{
|
||||
simBody1->clearSymmetricContact();
|
||||
symetricContact = verticalSymetricContact = false;
|
||||
} else if (!simBody1->isVerticalContact())
|
||||
verticalSymetricContact = false;
|
||||
} else
|
||||
symetricContact = verticalSymetricContact = false;
|
||||
}
|
||||
|
||||
if ( normalVel >= 0.0f && ( !symetricContact || age == 0 ) ) // body0 and body1 are separating
|
||||
return false;
|
||||
|
||||
if (!impulseComputed)
|
||||
{
|
||||
age++;
|
||||
// In world space:
|
||||
// skew = toSkewSymmetic(relativeContactPosition)
|
||||
//
|
||||
// a cross b = skew * b
|
||||
// b cross a = -a cross b = -skew * b
|
||||
// deltaVel = (-skew * inverseInertia * skew + diag(massRecip) ) * Impulse;
|
||||
|
||||
inverseMass = simBody0->getMassRecip();
|
||||
Matrix3 contactPositionSkew = Math::toSkewSymmetric(params.position - simBody0->getPV().position.translation);
|
||||
deltaVelPerUnitImpulse = -contactPositionSkew * simBody0->getInverseInertiaInWorld() * contactPositionSkew;
|
||||
|
||||
if (simBody1)
|
||||
{
|
||||
inverseMass += simBody1->getMassRecip();
|
||||
contactPositionSkew = Math::toSkewSymmetric(params.position - simBody1->getPV().position.translation);
|
||||
deltaVelPerUnitImpulse -= contactPositionSkew * simBody1->getInverseInertiaInWorld() * contactPositionSkew;
|
||||
}
|
||||
|
||||
deltaVelPerUnitImpulse += Math::fromDiagonal(Vector3(inverseMass, inverseMass, inverseMass));
|
||||
|
||||
bool revertable = deltaVelPerUnitImpulse.inverse(impulsePerUnitDeltaVel, 1e-20);
|
||||
RBXASSERT(revertable);
|
||||
}
|
||||
|
||||
float desiredDeltaVelocity = -normalVel; // Cancel the entrance relative velocity
|
||||
|
||||
float velocityThisFrame = simBody0->getMassRecip() * simBody0->getImpulseLast().dot(params.normal);
|
||||
if (simBody1)
|
||||
velocityThisFrame -= simBody1->getMassRecip() * simBody1->getImpulseLast().dot(params.normal);
|
||||
|
||||
if (!impulseComputed)
|
||||
{
|
||||
reboundVelocity = 0.0f;
|
||||
if (-normalVel >= REST_THRESHOLD && !isRestingContact() && !symetricContact &&
|
||||
velocityThisFrame >= 0.0f && velocityThisFrame <= -normalVel)
|
||||
{
|
||||
// Cancel the velocity entering this frame and the velocity just accumulated in this frame
|
||||
// Rebound only the velocity entering this frame
|
||||
float entranceVelocity = normalVel + velocityThisFrame;
|
||||
RBXASSERT(entranceVelocity <= 0.0f);
|
||||
// only rebounce the entrance velocity
|
||||
reboundVelocity = -contactParams.kNeg / contactParams.kSpring * entranceVelocity;
|
||||
RBXASSERT(reboundVelocity >= 0.0f);
|
||||
}
|
||||
|
||||
float penetrationBias = std::max(0.0f, -params.length - overlapGoal() * 2.0f) / Constants::freeFallDt() * PENETRATION_BIAS_WEIGHT;
|
||||
if (reboundVelocity < penetrationBias)
|
||||
reboundVelocity = penetrationBias;
|
||||
|
||||
penetrationVelocity = std::max(1.0f, normalVelSize);
|
||||
impulseComputed = true;
|
||||
}
|
||||
desiredDeltaVelocity += reboundVelocity;
|
||||
|
||||
residualVelocity += fabs(desiredDeltaVelocity / penetrationVelocity);
|
||||
|
||||
Vector3 velocityToKill = desiredDeltaVelocity * params.normal - perpVel;
|
||||
Vector3 impulse = impulsePerUnitDeltaVel * velocityToKill;
|
||||
|
||||
if (!verticalSymetricContact)
|
||||
{
|
||||
// Check static friction
|
||||
float impulseNormalSize = impulse.dot(params.normal);
|
||||
Vector3 impulseTangent = impulse - impulseNormalSize * params.normal;
|
||||
velocityThisFrame = fabs(velocityThisFrame);
|
||||
impulseNormalSize = fabs(impulseNormalSize);
|
||||
float impulseThisFrame = velocityThisFrame / inverseMass;
|
||||
float impulseTangentMax = std::max(impulseNormalSize, impulseThisFrame) * contactParams.kFriction;
|
||||
if (impulseTangent.squaredMagnitude() > impulseTangentMax * impulseTangentMax)
|
||||
{
|
||||
// Switch to the dynamic friction
|
||||
// deltaVel dot Normal = (-skew * inverseInertia * skew + diag(massRecip) ) *
|
||||
// ImpulseNormalSize * (Normal + kFriction * Tangent) dot Normal
|
||||
Vector3 impulseDirection = params.normal + contactParams.kFriction * FRICTION_GAIN * impulseTangent.direction();
|
||||
Vector3 VelDelta = deltaVelPerUnitImpulse * impulseDirection;
|
||||
if (desiredDeltaVelocity > 0)
|
||||
desiredDeltaVelocity = std::max(desiredDeltaVelocity, velocityThisFrame);
|
||||
else
|
||||
desiredDeltaVelocity = std::min(desiredDeltaVelocity, -velocityThisFrame);
|
||||
impulse = desiredDeltaVelocity / VelDelta.dot(params.normal) * impulseDirection;
|
||||
}
|
||||
}
|
||||
|
||||
simBody0->applyImpulse(-impulse, params.position);
|
||||
|
||||
if (simBody1)
|
||||
simBody1->applyImpulse(impulse, params.position);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void ContactConnector::applyContactPointForSymmetryDetection(SimBody* simBody0, SimBody* simBody1, const PairParams& params, float direction)
|
||||
{
|
||||
float massRecipSum = simBody0->getMassRecip();
|
||||
if (simBody1)
|
||||
massRecipSum += simBody1->getMassRecip();
|
||||
Vector3 penetrationForce = params.normal * params.length; // force applied on body0 by body1
|
||||
|
||||
float weight = simBody0->getMassRecip() / massRecipSum;
|
||||
Vector3 force = penetrationForce * weight;
|
||||
if (force.squaredMagnitude() > 1e-12)
|
||||
simBody0->accumulatePenetrationForce(force * direction, params.position);
|
||||
|
||||
if (simBody1)
|
||||
{
|
||||
weight = simBody1->getMassRecip() / massRecipSum;
|
||||
force = -penetrationForce * weight;
|
||||
if (force.squaredMagnitude() > 1e-12)
|
||||
simBody1->accumulatePenetrationForce(force * direction, params.position);
|
||||
}
|
||||
}
|
||||
|
||||
void ContactConnector::updateContactPoint()
|
||||
{
|
||||
if (!isContact())
|
||||
return;
|
||||
|
||||
if (contactPoint == oldContactPoint)
|
||||
return;
|
||||
|
||||
SimBody* simBody0 = NULL;
|
||||
SimBody* simBody1 = NULL;
|
||||
PairParams params = oldContactPoint;
|
||||
|
||||
if (getReordedSimBody(simBody0, simBody1, params))
|
||||
applyContactPointForSymmetryDetection(simBody0, simBody1, params, -1.0f);
|
||||
|
||||
params = contactPoint;
|
||||
if (getReordedSimBody(simBody0, simBody1, params))
|
||||
applyContactPointForSymmetryDetection(simBody0, simBody1, params, 1.0f);
|
||||
oldContactPoint = contactPoint;
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
void BallBallConnector::updateContactPoint()
|
||||
{
|
||||
const Vector3& p0 = geoPair.body0->getPosFast();
|
||||
contactPoint.normal = geoPair.body1->getPosFast() - p0;
|
||||
|
||||
contactPoint.length = contactPoint.normal.unitize() - radiusSum;
|
||||
contactPoint.position = p0 + radius0 * contactPoint.normal;
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
void BallBlockConnector::updateContactPoint()
|
||||
{
|
||||
if (geoPairType == BALL_PLANE_PAIR) {
|
||||
computeBallPlane(contactPoint);
|
||||
}
|
||||
else if (geoPairType == BALL_EDGE_PAIR) {
|
||||
computeBallEdge(contactPoint);
|
||||
}
|
||||
else {
|
||||
computeBallPoint(contactPoint);
|
||||
}
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
void BallBlockConnector::computeBallPlane(PairParams& params)
|
||||
{
|
||||
const Vector3& p0 = geoPair.body0->getPosFast();
|
||||
const CoordinateFrame& c1 = geoPair.body1->getCoordinateFrameFast();
|
||||
Vector3 p1 = c1.pointToWorldSpace(offset1);
|
||||
|
||||
params.normal = -Math::getWorldNormal(normalId1, c1);
|
||||
|
||||
Vector3 delta = p1 - p0; // vector from p1 to p0, edge point to center of ball
|
||||
Vector3 centerToPlanePoint = params.normal * params.normal.dot(delta);
|
||||
if( FFlag::BallBlockNarrowphaseFixEnabled )
|
||||
{
|
||||
params.position = p0 + radius0 * params.normal;
|
||||
}
|
||||
else
|
||||
{
|
||||
params.position = p0 + centerToPlanePoint;
|
||||
}
|
||||
|
||||
params.length = centerToPlanePoint.length() - radius0;
|
||||
}
|
||||
|
||||
void BallBlockConnector::computeBallEdge(PairParams& params)
|
||||
{
|
||||
const Vector3& p0 = geoPair.body0->getPosFast();
|
||||
const CoordinateFrame& c1 = geoPair.body1->getCoordinateFrameFast();
|
||||
|
||||
Vector3 p1 = c1.pointToWorldSpace(offset1);
|
||||
|
||||
Vector3 edgeNormal = Math::getWorldNormal(normalId1, c1);
|
||||
Vector3 delta = p1 - p0; // vector from p0 to p1, center of ball to edge point
|
||||
Vector3 projection = edgeNormal * edgeNormal.dot(delta);
|
||||
if( FFlag::BallBlockNarrowphaseFixEnabled )
|
||||
{
|
||||
params.normal = delta - projection; // right now normal is not unitized
|
||||
params.length = params.normal.unitize() - radius0;
|
||||
params.position = p0 + radius0 * params.normal;
|
||||
}
|
||||
else
|
||||
{
|
||||
params.normal = delta - projection; // right now normal is not unitized
|
||||
params.position = p0 + params.normal;
|
||||
params.length = params.normal.unitize() - radius0;
|
||||
}
|
||||
}
|
||||
|
||||
void BallBlockConnector::computeBallPoint(PairParams& params)
|
||||
{
|
||||
const Vector3& pCenter = geoPair.body0->getPosFast();
|
||||
params.position = geoPair.body1->getCoordinateFrameFast().pointToWorldSpace(offset1);
|
||||
params.normal = params.position - pCenter;
|
||||
params.length = params.normal.unitize() - radius0;
|
||||
if( FFlag::BallBlockNarrowphaseFixEnabled )
|
||||
{
|
||||
params.position = params.position - params.length * params.normal;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,646 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/Kernel.h"
|
||||
#include "V8Kernel/KernelData.h"
|
||||
#include "V8Kernel/Constants.h"
|
||||
#include "V8Kernel/Connector.h"
|
||||
#include "V8Kernel/ContactConnector.h"
|
||||
#include "rbx/Debug.h"
|
||||
#include "Util/Profiling.h"
|
||||
#include "util/Units.h"
|
||||
#include "rbx/rbxTime.h"
|
||||
|
||||
#include "rbx/Profiler.h"
|
||||
|
||||
FASTFLAGVARIABLE(UsePGSSolver, false)
|
||||
|
||||
namespace RBX {
|
||||
|
||||
int Kernel::numKernels = 0;
|
||||
|
||||
Kernel::Kernel(IStage* upstream)
|
||||
: IStage(upstream, NULL)
|
||||
, inStepCode(false)
|
||||
, profilingKernelBodies(new Profiling::CodeProfiler("Kernel Bodies"))
|
||||
, profilingKernelConnectors(new Profiling::CodeProfiler("Kernel Connectors"))
|
||||
, kernelData(new KernelData())
|
||||
, maxBodies(0)
|
||||
, error(0.0f)
|
||||
, maxError(0.0f)
|
||||
, numLastIterations(0)
|
||||
, numOfMaxIterations(0)
|
||||
, usingPGSSolver(false)
|
||||
{
|
||||
numKernels++;
|
||||
}
|
||||
|
||||
|
||||
|
||||
Kernel::~Kernel()
|
||||
{
|
||||
numKernels--;
|
||||
|
||||
RBXASSERT(!inStepCode);
|
||||
delete kernelData;
|
||||
}
|
||||
|
||||
bool Kernel::validateConnector(Connector* connector) const
|
||||
{
|
||||
bool oneInKernel = validateConnectorBody(connector->getBody(Connector::body0));
|
||||
oneInKernel = validateConnectorBody(connector->getBody(Connector::body1)) || oneInKernel;
|
||||
return oneInKernel;
|
||||
}
|
||||
|
||||
bool Kernel::validateConnectorBody(Body* b) const
|
||||
{
|
||||
if (b)
|
||||
{
|
||||
if (b->getRootSimBody()->isInKernel())
|
||||
{
|
||||
return true;
|
||||
} else if (b->isLeafBody())
|
||||
{
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return false;
|
||||
}
|
||||
|
||||
bool Kernel::validateBody(Body* b)
|
||||
{
|
||||
RBXASSERT_VERY_FAST(!inStepCode);
|
||||
RBXASSERT_FISHING(Math::longestVector3Component(b->getBranchForce()) < 1e9f);
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
void Kernel::insertBody(Body* b)
|
||||
{
|
||||
RBXASSERT(b->getRootSimBody()->getDt() == 0.0f);
|
||||
|
||||
kernelData->insertBody(b); //bodies.fastAppend(b);
|
||||
RBXASSERT(validateBody(b));
|
||||
maxBodies = std::max(maxBodies, numBodies());
|
||||
|
||||
if (usingPGSSolver)
|
||||
pgsSolver.addSimBody( b->getRootSimBody(), !b->getCanThrottle() );
|
||||
}
|
||||
|
||||
|
||||
void Kernel::removeBody(Body* b)
|
||||
{
|
||||
if (usingPGSSolver)
|
||||
pgsSolver.removeSimBody( b->getRootSimBody() );
|
||||
|
||||
RBXASSERT(!inStepCode);
|
||||
RBXASSERT((!b->getRootSimBody()->isContactBody() && !b->getRootSimBody()->isFreeFallBody()) ||
|
||||
Math::fuzzyEq(b->getRootSimBody()->getDt(), Constants::freeFallDt(), 1e-6f));
|
||||
RBXASSERT((b->getRootSimBody()->isContactBody() || b->getRootSimBody()->isFreeFallBody()) ||
|
||||
Math::fuzzyEq(b->getRootSimBody()->getDt(), Constants::kernelDt(), 1e-6f));
|
||||
kernelData->removeBody(b); // kernelData->bodies.fastRemove(b);
|
||||
|
||||
}
|
||||
|
||||
void Kernel::insertPoint(Point* p)
|
||||
{
|
||||
RBXASSERT(!inStepCode);
|
||||
kernelData->points.fastAppend(p);
|
||||
}
|
||||
|
||||
void Kernel::insertConnector(Connector* c)
|
||||
{
|
||||
RBXASSERT(!inStepCode);
|
||||
kernelData->addConnector(c, usingPGSSolver);
|
||||
}
|
||||
|
||||
void Kernel::removePoint(Point* p)
|
||||
{
|
||||
RBXASSERT(!inStepCode);
|
||||
kernelData->points.fastRemove(p);
|
||||
}
|
||||
|
||||
void Kernel::removeConnector(Connector* c)
|
||||
{
|
||||
RBXASSERT(!inStepCode);
|
||||
kernelData->removeConnector(c);
|
||||
}
|
||||
|
||||
int Kernel::numFreeFallBodies() const {return kernelData->freeFallBodies.size();}
|
||||
int Kernel::numRealTimeBodies() const {return kernelData->realTimeBodies.size();}
|
||||
int Kernel::numJointBodies() const {return kernelData->jointBodies.size();}
|
||||
int Kernel::numContactBodies() const {return kernelData->contactBodies.size();}
|
||||
int Kernel::numLeafBodies() const {return kernelData->leafBodies.size();}
|
||||
int Kernel::numPoints() const {return kernelData->points.size();}
|
||||
int Kernel::numHumanoidConnectors() const {return kernelData->humanoidConnectors.size();}
|
||||
int Kernel::numRealTimeConnectors() const {return kernelData->realTimeConnectors.size();}
|
||||
int Kernel::numSecondPassConnectors() const {return kernelData->secondPassConnectors.size();}
|
||||
int Kernel::numJointConnectors() const {return kernelData->jointConnectors.size();}
|
||||
int Kernel::numBuoyancyConnectors() const {return kernelData->buoyancyConnectors.size();}
|
||||
int Kernel::numContactConnectors() const {return kernelData->contactConnectors.size();}
|
||||
int Kernel::numConnectors() const {return numRealTimeConnectors() + numSecondPassConnectors() + numJointConnectors() + numContactConnectors() + numBuoyancyConnectors();}
|
||||
|
||||
// double up on points if same body, position....
|
||||
Point* Kernel::newPoint(Body* _body, const Vector3& worldPos)
|
||||
{
|
||||
RBXASSERT(!inStepCode);
|
||||
|
||||
Point* tempPoint = new Point(_body);
|
||||
tempPoint->setWorldPos(worldPos);
|
||||
|
||||
return searchForDuplicatePoint(tempPoint);
|
||||
}
|
||||
|
||||
Point* Kernel::newPointLocal(class Body* _body, const Vector3& localPos)
|
||||
{
|
||||
RBXASSERT(!inStepCode);
|
||||
|
||||
Point* tempPoint = new Point(_body);
|
||||
tempPoint->setLocalPos(localPos);
|
||||
|
||||
return searchForDuplicatePoint(tempPoint);
|
||||
}
|
||||
|
||||
Point* Kernel::searchForDuplicatePoint(Point* tempPoint)
|
||||
{
|
||||
for (int i = 0; i < kernelData->points.size(); i++) {
|
||||
if (Point::sameBodyAndOffset(*tempPoint, *kernelData->points[i])) {
|
||||
kernelData->points[i]->numOwners++;
|
||||
delete tempPoint;
|
||||
return kernelData->points[i];
|
||||
}
|
||||
}
|
||||
|
||||
insertPoint(tempPoint);
|
||||
return tempPoint;
|
||||
}
|
||||
|
||||
|
||||
|
||||
void Kernel::deletePoint(class Point* _point)
|
||||
{
|
||||
RBXASSERT(!inStepCode);
|
||||
|
||||
if (_point) {
|
||||
_point->numOwners--;
|
||||
if (_point->numOwners == 0) {
|
||||
removePoint(_point);
|
||||
delete _point;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Kernel::step(bool throttling, int numThreads, boost::uint64_t debugTime)
|
||||
{
|
||||
RBXASSERT(!inStepCode);
|
||||
inStepCode = true;
|
||||
|
||||
if (throttling)
|
||||
{
|
||||
preStepThrottled();
|
||||
stepWorldThrottled(debugTime);
|
||||
} else
|
||||
{
|
||||
preStep();
|
||||
stepWorld(debugTime);
|
||||
}
|
||||
inStepCode = false;
|
||||
}
|
||||
|
||||
void Kernel::preStep()
|
||||
{
|
||||
|
||||
IndexArray<SimBody, &SimBody::getRealTimeBodyIndex>& realTimeBodies(kernelData->realTimeBodies); // humanoid bodies that are not throttle-able
|
||||
IndexArray<SimBody, &SimBody::getJointBodyIndex>& jointBodies(kernelData->jointBodies); // bodies with joint connectors
|
||||
IndexArray<SimBody, &SimBody::getContactBodyIndex>& contactBodies(kernelData->contactBodies); // bodies with contact connectors
|
||||
IndexArray<Connector, &Connector::getContactIndex>& contactConnectors(kernelData->contactConnectors);
|
||||
|
||||
{
|
||||
RBX::Profiling::Mark mark(*profilingKernelBodies, false);
|
||||
for (int i = 0; i < realTimeBodies.size(); ++i)
|
||||
realTimeBodies[i]->updateIfDirty();
|
||||
for (int i = 0; i < jointBodies.size(); ++i)
|
||||
jointBodies[i]->updateIfDirty();
|
||||
|
||||
if (!usingPGSSolver)
|
||||
{
|
||||
for (int i = 0; i < contactBodies.size(); ++i)
|
||||
{
|
||||
SimBody* simBody = contactBodies[i];
|
||||
simBody->updateIfDirty();
|
||||
if (simBody->hasExternalForceOrImpulse())
|
||||
{
|
||||
contactBodies.fastRemove(simBody);
|
||||
realTimeBodies.fastAppend(simBody);
|
||||
kernelData->addLeafBodies(simBody->getBody());
|
||||
simBody->setDt(Constants::kernelDt());
|
||||
--i;
|
||||
} else
|
||||
{
|
||||
simBody->updateSymmetricContactState();
|
||||
simBody->stepVelocity();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
{
|
||||
RBX::Profiling::Mark mark(*profilingKernelConnectors, false);
|
||||
|
||||
if (!usingPGSSolver)
|
||||
{
|
||||
// TODO: optimize this
|
||||
for (int i = 0; i < contactConnectors.size(); ++i)
|
||||
{
|
||||
ContactConnector* conn = rbx_static_cast<ContactConnector*>(contactConnectors[i]);
|
||||
conn->clearImpulseComputed();
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Kernel::preStepThrottled()
|
||||
{
|
||||
const IndexArray<SimBody, &SimBody::getRealTimeBodyIndex>& realTimeBodies(kernelData->realTimeBodies); // humanoid bodies that are not throttle-able
|
||||
{
|
||||
RBX::Profiling::Mark mark(*profilingKernelBodies, false);
|
||||
// forces update of Cofm as well
|
||||
for (int i = 0; i < realTimeBodies.size(); ++i)
|
||||
realTimeBodies[i]->updateIfDirty();
|
||||
}
|
||||
}
|
||||
|
||||
void Kernel::stepWorld( boost::uint64_t debugTime )
|
||||
{
|
||||
RBXPROFILER_SCOPE("Physics", "Kernel::stepWorld");
|
||||
|
||||
IndexArray<SimBody, &SimBody::getFreeFallBodyIndex>& freeFallBodies(kernelData->freeFallBodies);// bodies with no connectors
|
||||
IndexArray<SimBody, &SimBody::getRealTimeBodyIndex>& realTimeBodies(kernelData->realTimeBodies);// humanoid bodies that are not throttle-able
|
||||
IndexArray<SimBody, &SimBody::getContactBodyIndex>& contactBodies(kernelData->contactBodies); // bodies with contact connectors but no joint connectors
|
||||
const IndexArray<SimBody, &SimBody::getJointBodyIndex>& jointBodies(kernelData->jointBodies); // bodies with joint connectors
|
||||
const IndexArray<Body, &Body::getLeafBodyIndex>& leafBodies(kernelData->leafBodies); // need update PV every step, NOT in kernel!
|
||||
const IndexArray<Point, &Point::getKernelIndex>& points(kernelData->points);
|
||||
const IndexArray<Connector, &Connector::getHumanoidIndex>& humanoidConnectors(kernelData->humanoidConnectors); // humanoid
|
||||
const IndexArray<Connector, &Connector::getRealTimeIndex>& realtimeConnectors(kernelData->realTimeConnectors); // connectors on humanoid body parts
|
||||
const IndexArray<Connector, &Connector::getSecondPassIndex>& secondPassConnectors(kernelData->secondPassConnectors);// kernel joints
|
||||
const IndexArray<Connector, &Connector::getJointIndex>& jointConnectors(kernelData->jointConnectors);
|
||||
const IndexArray<Connector, &Connector::getBuoyancyIndex>& buoyancyConnectors(kernelData->buoyancyConnectors);
|
||||
const IndexArray<Connector, &Connector::getContactIndex>& contactConnectors(kernelData->contactConnectors);
|
||||
|
||||
if( usingPGSSolver )
|
||||
{
|
||||
std::vector< ContactConnector* > allContactConnectors;
|
||||
|
||||
// humanoid connectors
|
||||
for (int j = 0; j < humanoidConnectors.size(); ++j)
|
||||
humanoidConnectors[j]->computeForce(false);
|
||||
|
||||
// buoyancy
|
||||
for (int j = 0; j < buoyancyConnectors.size(); ++j)
|
||||
buoyancyConnectors[j]->computeForce(false);
|
||||
|
||||
for (int j = 0; j < secondPassConnectors.size(); ++j) {
|
||||
RBXASSERT(validateConnector(secondPassConnectors[j]));
|
||||
secondPassConnectors[j]->computeForce(false);
|
||||
}
|
||||
|
||||
// contact with humanoids
|
||||
for (int j = 0; j < realtimeConnectors.size(); ++j)
|
||||
{
|
||||
ContactConnector* contact = static_cast< ContactConnector* >( realtimeConnectors[j] );
|
||||
allContactConnectors.push_back( contact );
|
||||
}
|
||||
|
||||
// all standard contacts
|
||||
for (int j = 0; j < contactConnectors.size(); ++j)
|
||||
{
|
||||
ContactConnector* contact = static_cast< ContactConnector* >( contactConnectors[j] );
|
||||
allContactConnectors.push_back( contact );
|
||||
}
|
||||
|
||||
// joints
|
||||
for (int j = 0; j < jointConnectors.size(); ++j)
|
||||
{
|
||||
if (jointConnectors[j]->getConnectorKernelType() == Connector::CONTACT)
|
||||
{
|
||||
ContactConnector* contact = static_cast< ContactConnector* >( jointConnectors[j] );
|
||||
allContactConnectors.push_back( contact );
|
||||
}
|
||||
}
|
||||
|
||||
pgsSolver.solve( allContactConnectors, Constants::worldDt(), debugTime, false );
|
||||
}
|
||||
else // legacy solver
|
||||
{
|
||||
/////////////////////////// Free Fall Solver ///////////////////////////
|
||||
{
|
||||
RBX::Profiling::Mark mark(*profilingKernelBodies, false);
|
||||
for (int i = 0; i < freeFallBodies.size(); ++i)
|
||||
{
|
||||
SimBody* simBody = freeFallBodies[i];
|
||||
simBody->updateIfDirty();
|
||||
if (simBody->hasExternalForceOrImpulse())
|
||||
{
|
||||
simBody->updateAngMomentum();
|
||||
freeFallBodies.fastRemove(simBody);
|
||||
realTimeBodies.fastAppend(simBody);
|
||||
simBody->setDt(Constants::kernelDt());
|
||||
--i;
|
||||
} else
|
||||
{
|
||||
simBody->stepFreeFall();
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (contactConnectors.size() > 0)
|
||||
{
|
||||
RBX::Profiling::Mark mark(*profilingKernelConnectors, false);
|
||||
float residualVelocity = 0.0f;
|
||||
int i;
|
||||
float tolerance = Constants::impulseSolverAccuracy() *
|
||||
(contactBodies.size() / Constants::impulseSolverAccuracyScalar() + 1);
|
||||
|
||||
for (i = 0; i < Constants::impulseSolverMaxIterations(); ++i)
|
||||
{
|
||||
residualVelocity = 0.0f;
|
||||
for (int j = 0; j < contactConnectors.size(); ++j)
|
||||
{
|
||||
RBXASSERT(validateConnector(contactConnectors[j]));
|
||||
contactConnectors[j]->computeImpulse(residualVelocity);
|
||||
}
|
||||
residualVelocity /= contactConnectors.size();
|
||||
if (residualVelocity < tolerance)
|
||||
{
|
||||
++i;
|
||||
break;
|
||||
}
|
||||
}
|
||||
numLastIterations = i;
|
||||
if (numLastIterations > numOfMaxIterations)
|
||||
numOfMaxIterations = numLastIterations;
|
||||
error = residualVelocity;
|
||||
if (error > maxError)
|
||||
maxError = error;
|
||||
}
|
||||
|
||||
{
|
||||
RBX::Profiling::Mark mark(*profilingKernelBodies, false);
|
||||
for (int i = 0; i < contactBodies.size(); ++i)
|
||||
contactBodies[i]->stepPosition();
|
||||
}
|
||||
|
||||
/////////////////////////// Joint Solver //////////////////////////////
|
||||
for (int i = 0; i < Constants::kernelStepsPerWorldStep(); i++)
|
||||
{
|
||||
{
|
||||
RBX::Profiling::Mark mark(*profilingKernelConnectors, false);
|
||||
|
||||
for (int j = 0; j < points.size(); ++j)
|
||||
points[j]->step();
|
||||
|
||||
for (int j = 0; j < realtimeConnectors.size(); ++j) {
|
||||
RBXASSERT(validateConnector(realtimeConnectors[j]));
|
||||
realtimeConnectors[j]->computeForce(false);
|
||||
}
|
||||
|
||||
for (int j = 0; j < jointConnectors.size(); ++j) {
|
||||
RBXASSERT(validateConnector(jointConnectors[j]));
|
||||
jointConnectors[j]->computeForce(false);
|
||||
}
|
||||
|
||||
for (int j = 0; j < points.size(); ++j)
|
||||
points[j]->forceToBody();
|
||||
|
||||
for (int j = 0; j < humanoidConnectors.size(); ++j)
|
||||
humanoidConnectors[j]->computeForce(false);
|
||||
|
||||
for (int j = 0; j < secondPassConnectors.size(); ++j) {
|
||||
RBXASSERT(validateConnector(secondPassConnectors[j]));
|
||||
secondPassConnectors[j]->computeForce(false);
|
||||
}
|
||||
}
|
||||
{
|
||||
RBX::Profiling::Mark mark(*profilingKernelBodies, false);
|
||||
|
||||
for (int j = 0; j < realTimeBodies.size(); ++j)
|
||||
realTimeBodies[j]->step();
|
||||
|
||||
for (int j = 0; j < jointBodies.size(); ++j)
|
||||
jointBodies[j]->step();
|
||||
|
||||
// TODO: Remove this later
|
||||
const int leafBodiesSize = leafBodies.size();
|
||||
// TODO: Parallel? Only if using getPV with lock protectiob
|
||||
for (int j = 0; j < leafBodiesSize; ++j)
|
||||
leafBodies[j]->getPvUnsafe(); // forces Update
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
void Kernel::stepWorldThrottled( boost::uint64_t debugTime )
|
||||
{
|
||||
RBXPROFILER_SCOPE("Physics", "Kernel::stepWorldThrottled");
|
||||
|
||||
const IndexArray<SimBody, &SimBody::getRealTimeBodyIndex>& realtimeBodies(kernelData->realTimeBodies); // humanoid bodies that are not throttle-able
|
||||
const IndexArray<Body, &Body::getLeafBodyIndex>& leafBodies(kernelData->leafBodies); // need update PV every step, NOT in kernel!
|
||||
const IndexArray<Point, &Point::getKernelIndex>& points(kernelData->points);
|
||||
const IndexArray<Connector, &Connector::getRealTimeIndex>& realtimeConnectors(kernelData->realTimeConnectors); // connectors on humanoid body parts
|
||||
const IndexArray<Connector, &Connector::getHumanoidIndex>& humanoidConnectors(kernelData->humanoidConnectors); // humanoids
|
||||
|
||||
if( usingPGSSolver )
|
||||
{
|
||||
// Compute forces
|
||||
for (int j = 0; j < humanoidConnectors.size(); ++j)
|
||||
humanoidConnectors[j]->computeForce(false);
|
||||
|
||||
// Gather the connectors
|
||||
std::vector< ContactConnector* > allContactConnectors;
|
||||
for (int j = 0; j < realtimeConnectors.size(); ++j)
|
||||
{
|
||||
ContactConnector* contact = static_cast< ContactConnector* >( realtimeConnectors[j] );
|
||||
allContactConnectors.push_back( contact );
|
||||
}
|
||||
|
||||
pgsSolver.solve( allContactConnectors, Constants::worldDt(), debugTime, true );
|
||||
}
|
||||
else
|
||||
{
|
||||
/////////////////////////// Joint Solve //////////////////////////////
|
||||
for (int i = 0; i < Constants::kernelStepsPerWorldStep(); i++)
|
||||
{
|
||||
{
|
||||
RBX::Profiling::Mark mark(*profilingKernelConnectors, false);
|
||||
|
||||
for (int j = 0; j < points.size(); ++j)
|
||||
points[j]->step();
|
||||
|
||||
for (int j = 0; j < realtimeConnectors.size(); ++j) {
|
||||
RBXASSERT(validateConnector(realtimeConnectors[j]));
|
||||
realtimeConnectors[j]->computeForce(false);
|
||||
}
|
||||
|
||||
for (int j = 0; j < points.size(); ++j)
|
||||
points[j]->forceToBody();
|
||||
|
||||
for (int j = 0; j < humanoidConnectors.size(); ++j)
|
||||
humanoidConnectors[j]->computeForce(false);
|
||||
}
|
||||
{
|
||||
RBX::Profiling::Mark mark(*profilingKernelBodies, false);
|
||||
|
||||
for (int j = 0; j < realtimeBodies.size(); ++j)
|
||||
realtimeBodies[j]->step();
|
||||
|
||||
// TODO: Remove this later
|
||||
const int leafBodiesSize = leafBodies.size();
|
||||
|
||||
// TODO: Parallel? Only if using getPV with lock protectiob
|
||||
for (int j = 0; j < leafBodiesSize; ++j)
|
||||
leafBodies[j]->getPvUnsafe(); // forces Update
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
float Kernel::connectorSpringEnergy() const
|
||||
{
|
||||
float springPotential = 0.0;
|
||||
|
||||
for (int j = 0; j < kernelData->secondPassConnectors.size(); ++j)
|
||||
springPotential += kernelData->secondPassConnectors[j]->potentialEnergy();
|
||||
|
||||
for (int j = 0; j < kernelData->realTimeConnectors.size(); ++j)
|
||||
springPotential += kernelData->realTimeConnectors[j]->potentialEnergy();
|
||||
|
||||
for (int j = 0; j < kernelData->jointConnectors.size(); ++j)
|
||||
springPotential += kernelData->jointConnectors[j]->potentialEnergy();
|
||||
|
||||
for (int j = 0; j < kernelData->contactConnectors.size(); ++j)
|
||||
springPotential += kernelData->contactConnectors[j]->potentialEnergy();
|
||||
|
||||
|
||||
return springPotential;
|
||||
}
|
||||
|
||||
float Kernel::bodyPotentialEnergy() const
|
||||
{
|
||||
float gravitationalPotential = 0.0;
|
||||
|
||||
for (int j = 0; j < kernelData->realTimeBodies.size(); ++j)
|
||||
gravitationalPotential += kernelData->realTimeBodies[j]->getBody()->potentialEnergy();
|
||||
|
||||
for (int j = 0; j < kernelData->freeFallBodies.size(); ++j)
|
||||
gravitationalPotential += kernelData->freeFallBodies[j]->getBody()->potentialEnergy();
|
||||
|
||||
for (int j = 0; j < kernelData->jointBodies.size(); ++j)
|
||||
gravitationalPotential += kernelData->jointBodies[j]->getBody()->potentialEnergy();
|
||||
|
||||
for (int j = 0; j < kernelData->contactBodies.size(); ++j)
|
||||
gravitationalPotential += kernelData->contactBodies[j]->getBody()->potentialEnergy();
|
||||
|
||||
return gravitationalPotential;
|
||||
}
|
||||
|
||||
float Kernel::bodyKineticEnergy() const
|
||||
{
|
||||
float kineticEnergy = 0.0;
|
||||
|
||||
for (int j = 0; j < kernelData->realTimeBodies.size(); ++j)
|
||||
kineticEnergy += kernelData->realTimeBodies[j]->getBody()->kineticEnergy();
|
||||
|
||||
for (int j = 0; j < kernelData->freeFallBodies.size(); ++j)
|
||||
kineticEnergy += kernelData->freeFallBodies[j]->getBody()->kineticEnergy();
|
||||
|
||||
for (int j = 0; j < kernelData->jointBodies.size(); ++j)
|
||||
kineticEnergy += kernelData->jointBodies[j]->getBody()->kineticEnergy();
|
||||
|
||||
for (int j = 0; j < kernelData->contactBodies.size(); ++j)
|
||||
kineticEnergy += kernelData->contactBodies[j]->getBody()->kineticEnergy();
|
||||
return kineticEnergy;
|
||||
}
|
||||
|
||||
|
||||
void Kernel::report()
|
||||
{
|
||||
}
|
||||
|
||||
void Kernel::reportMemorySizes()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
int Kernel::fakeDeceptiveSolverIterations() const
|
||||
{
|
||||
int fakeMatrixSize = fakeDeceptiveMatrixSize() + 4;
|
||||
|
||||
return 1 + static_cast<int>(sqrt(sqrt(static_cast<float>(fakeMatrixSize))));
|
||||
}
|
||||
|
||||
|
||||
|
||||
int Kernel::fakeDeceptiveMatrixSize() const
|
||||
{
|
||||
return numConnectors() + (6 * numBodies());
|
||||
}
|
||||
|
||||
/////////////////////////////////////////////////////////////////////////////////////////
|
||||
//
|
||||
// Funny Physics
|
||||
void Kernel::stepWorldFunnyPhysics(int worldStepId)
|
||||
{
|
||||
if ((worldStepId % Constants::worldStepsPerUiStep()) == 0)
|
||||
{
|
||||
int seconds = worldStepId * Constants::worldDt();
|
||||
int phase = seconds % 4;
|
||||
Vector3 move;
|
||||
switch (phase)
|
||||
{
|
||||
case 0: move.x = 1.0f; break;
|
||||
case 1: move.y = 1.0f; break;
|
||||
case 2: move.x = -1.0f; break;
|
||||
case 3: move.y = -1.0f; break;
|
||||
default: break;
|
||||
}
|
||||
|
||||
move *= 0.4f;
|
||||
stepFunnyPhysics(move);
|
||||
}
|
||||
}
|
||||
|
||||
void Kernel::stepFunnyPhysicsBody(Body* b, const Vector3& move)
|
||||
{
|
||||
CoordinateFrame c = b->getCoordinateFrame();
|
||||
c.translation += move;
|
||||
Math::rotateAboutYGlobal(c, 0.01f);
|
||||
Math::orthonormalizeIfNecessary(c.rotation);
|
||||
b->setCoordinateFrame(c, *this);
|
||||
}
|
||||
|
||||
|
||||
void Kernel::stepFunnyPhysics(const Vector3& move)
|
||||
{
|
||||
const IndexArray<SimBody, &SimBody::getFreeFallBodyIndex>& freeFallBodies(kernelData->freeFallBodies); // bodies with no connectors
|
||||
const IndexArray<SimBody, &SimBody::getRealTimeBodyIndex>& realTimeBodies(kernelData->realTimeBodies); // humanoid bodies that are not throttle-able
|
||||
const IndexArray<SimBody, &SimBody::getJointBodyIndex>& jointBodies(kernelData->jointBodies); // bodies with joint connectors
|
||||
const IndexArray<SimBody, &SimBody::getContactBodyIndex>& contactBodies(kernelData->contactBodies); // bodies with contact connectors but no joint connectors
|
||||
|
||||
for (int i = 0; i < freeFallBodies.size(); ++i)
|
||||
stepFunnyPhysicsBody(freeFallBodies[i]->getBody(), move);
|
||||
|
||||
for (int i = 0; i < realTimeBodies.size(); ++i)
|
||||
stepFunnyPhysicsBody(realTimeBodies[i]->getBody(), move);
|
||||
|
||||
for (int i = 0; i < jointBodies.size(); ++i)
|
||||
stepFunnyPhysicsBody(jointBodies[i]->getBody(), move);
|
||||
|
||||
for (int i = 0; i < contactBodies.size(); ++i)
|
||||
stepFunnyPhysicsBody(contactBodies[i]->getBody(), move);
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,70 @@
|
||||
/* Copyright 2003-2006 ROBLOX Corporation, All Rights Reserved */
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/Link.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
|
||||
Link::Link()
|
||||
: body(NULL)
|
||||
, stateIndex(Body::getNextStateIndex())
|
||||
{
|
||||
}
|
||||
|
||||
Link::~Link()
|
||||
{
|
||||
}
|
||||
|
||||
void Link::dirty()
|
||||
{
|
||||
if (body)
|
||||
{
|
||||
RBXASSERT(body->getLink() == this);
|
||||
body->makeStateDirty();
|
||||
}
|
||||
}
|
||||
|
||||
const CoordinateFrame& Link::getChildInParent()
|
||||
{
|
||||
unsigned int parentState = body->getParent()->getStateIndex();
|
||||
|
||||
if (stateIndex != parentState)
|
||||
{
|
||||
computeChildInParent(childInParent);
|
||||
stateIndex = parentState;
|
||||
}
|
||||
return childInParent;
|
||||
}
|
||||
|
||||
void Link::reset(
|
||||
const CoordinateFrame& parentC,
|
||||
const CoordinateFrame& childC)
|
||||
{
|
||||
parentCoord = parentC;
|
||||
childCoord = childC;
|
||||
childCoordInverse = childC.inverse();
|
||||
}
|
||||
|
||||
|
||||
void RevoluteLink::computeChildInParent(CoordinateFrame& answer) const
|
||||
{
|
||||
CoordinateFrame rotatedParentCoord( Math::rotateAboutZ(parentCoord.rotation, jointAngle),
|
||||
parentCoord.translation);
|
||||
|
||||
CoordinateFrame::mul(rotatedParentCoord, childCoordInverse, answer);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void D6Link::computeChildInParent(CoordinateFrame& answer) const
|
||||
{
|
||||
CoordinateFrame rotatedParentCoord = parentCoord * offsetCFrame;
|
||||
|
||||
CoordinateFrame::mul(rotatedParentCoord, childCoordInverse, answer);
|
||||
}
|
||||
|
||||
|
||||
} // namespace
|
||||
|
||||
@@ -0,0 +1,164 @@
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/Pair.h"
|
||||
#include "V8Kernel/Constants.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
#include "Util/Math.h"
|
||||
|
||||
|
||||
namespace RBX {
|
||||
|
||||
|
||||
GeoPair::GeoPair()
|
||||
: body0(NULL)
|
||||
, body1(NULL)
|
||||
{}
|
||||
|
||||
|
||||
|
||||
void GeoPair::computePointPlane(PairParams& _params)
|
||||
{
|
||||
_params.position = body0->getCoordinateFrameFast().pointToWorldSpace(offset0);
|
||||
const CoordinateFrame& c1 = body1->getCoordinateFrameFast();
|
||||
Vector3 pPlane = c1.pointToWorldSpace(offset1);
|
||||
_params.normal = -Math::getWorldNormal(pairData.normalID1, c1);
|
||||
_params.length = _params.normal.dot(pPlane - _params.position); // length is negative when overlapping
|
||||
}
|
||||
|
||||
// using notation from ODE
|
||||
// Use the normal from the plane - planeID
|
||||
// b0 is an edge body
|
||||
// b1 is an edge body, and the planeNormal body
|
||||
// if (reversePolarity), this means that
|
||||
// the normal needs to be switched
|
||||
// because the points corresponds to opposite bodies
|
||||
// only need position of pa, the NON-PLANE edge
|
||||
|
||||
void GeoPair::computeEdgeEdgePlane(PairParams& _params)
|
||||
{
|
||||
const CoordinateFrame& ca = body0->getCoordinateFrameFast();
|
||||
const CoordinateFrame& cPlane = body1->getCoordinateFrameFast();
|
||||
|
||||
Vector3 pa = ca.pointToWorldSpace(offset0);
|
||||
Vector3 pPlane = cPlane.pointToWorldSpace(offset1);
|
||||
|
||||
Vector3 ua = Math::getWorldNormal(pairData.normalID0, ca);
|
||||
Vector3 ub = Math::getWorldNormal(pairData.normalID1, cPlane);
|
||||
|
||||
Vector3 p = pPlane - pa;
|
||||
float uaub = ua.dot(ub);
|
||||
float q1 = ua.dot(p);
|
||||
float q2 = -ub.dot(p);
|
||||
|
||||
float d = 1 - uaub*uaub;
|
||||
|
||||
_params.normal = -Math::getWorldNormal(pairData.planeID, cPlane);
|
||||
|
||||
if (d > 1e-5) { // work here
|
||||
float alpha = (q1 + uaub*q2) / d;
|
||||
if (fabs(alpha) > 6.0f)
|
||||
{
|
||||
alpha = 6.0f * Math::sign(alpha);
|
||||
}
|
||||
_params.position = pa + alpha * ua;
|
||||
_params.length = _params.normal.dot(pPlane - _params.position);
|
||||
}
|
||||
else {
|
||||
_params.position = pa;
|
||||
_params.length = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void GeoPair::computeEdgeEdgePlane2(PairParams& _params)
|
||||
{
|
||||
const CoordinateFrame& ca = body0->getCoordinateFrameFast();
|
||||
const CoordinateFrame& cPlane = body1->getCoordinateFrameFast();
|
||||
|
||||
Vector3 p1 = offset0;
|
||||
Vector3 p2 = ca.pointToObjectSpace(cPlane.pointToWorldSpace(offset1));
|
||||
|
||||
Vector3 u1 = ca.vectorToObjectSpace(Math::getWorldNormal(pairData.normalID0, ca));
|
||||
Vector3 u2 = ca.vectorToObjectSpace(Math::getWorldNormal(pairData.normalID1, cPlane));
|
||||
|
||||
_params.normal = -Math::getWorldNormal(pairData.planeID, cPlane);
|
||||
|
||||
// Effectively, if they're far enough from being parallel,
|
||||
if( fabs(fabs(u1.dot(u2)) - 1.0) > 1e-5 )
|
||||
{
|
||||
float a1 = (p1 - p2).dot(u1);
|
||||
float a2 = (p1 - p2).dot(u2);
|
||||
float b = u1.dot(u2);
|
||||
|
||||
float t1 = (b*a2 - a1*b*b)/(1 - b*b) - a1;
|
||||
float t2 = (a2 - a1*b)/(1 - b*b);
|
||||
|
||||
Vector3 body1ClosestPointInBody1 = p1 + t1*u1;
|
||||
|
||||
Vector3 body2ClosestPointInBody2 = cPlane.pointToObjectSpace(ca.pointToWorldSpace(p2 + t2*u2));
|
||||
float body1ClosestPointEdgeLocation = body1ClosestPointInBody1[pairData.normalID0 % 3];
|
||||
float body2ClosestPointEdgeLocation = body2ClosestPointInBody2[pairData.normalID1 % 3];
|
||||
|
||||
float eps = 0.05;
|
||||
if ((fabs(body1ClosestPointEdgeLocation) > 0.5 * edgeLength0 + eps) ||
|
||||
(fabs(body2ClosestPointEdgeLocation) > 0.5 * edgeLength1 + eps))
|
||||
{
|
||||
_params.position = ca.pointToWorldSpace(offset0);
|
||||
_params.length = 0.0f;
|
||||
}
|
||||
else
|
||||
{
|
||||
_params.position = ca.pointToWorldSpace(body1ClosestPointInBody1);
|
||||
_params.length = _params.normal.dot(cPlane.pointToWorldSpace(offset1) - _params.position);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
_params.position = ca.pointToWorldSpace(offset0);
|
||||
_params.length = 0.0f;
|
||||
}
|
||||
}
|
||||
|
||||
void GeoPair::computeEdgeEdge(PairParams& _params)
|
||||
{
|
||||
const CoordinateFrame& ca = body0->getCoordinateFrameFast();
|
||||
const CoordinateFrame& cb = body1->getCoordinateFrameFast();
|
||||
|
||||
Vector3 pa = ca.pointToWorldSpace(offset0);
|
||||
Vector3 pb = cb.pointToWorldSpace(offset1);
|
||||
|
||||
Vector3 ua = Math::getWorldNormal(pairData.normalID0, ca);
|
||||
Vector3 ub = Math::getWorldNormal(pairData.normalID1, cb);
|
||||
|
||||
Vector3 p = pb - pa;
|
||||
float uaub = ua.dot(ub);
|
||||
float q1 = ua.dot(p);
|
||||
float q2 = -ub.dot(p);
|
||||
float d = 1 - uaub*uaub;
|
||||
if (d > 1e-6f) {
|
||||
d = 1.0f / d;
|
||||
float alpha = (q1 + uaub*q2)*d;
|
||||
float beta = (uaub*q1 + q2)*d;
|
||||
pa = pa + alpha * ua;
|
||||
pb = pb + beta * ub;
|
||||
_params.position = 0.5 * (pa + pb);
|
||||
_params.normal = pa - pb;
|
||||
_params.length = -_params.normal.unitize();
|
||||
}
|
||||
else {
|
||||
_params.position = pa;
|
||||
_params.normal = ua;
|
||||
_params.length = 0.0; // hopefully will blow out;
|
||||
}
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
|
||||
// Randomized Locations for hackflags
|
||||
namespace RBX
|
||||
{
|
||||
namespace Security
|
||||
{
|
||||
unsigned int hackFlag12 = 0;
|
||||
};
|
||||
};
|
||||
@@ -0,0 +1,50 @@
|
||||
/* Copyright 2003-2007 ROBLOX Corporation, All Rights Reserved */
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/Point.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
|
||||
|
||||
namespace RBX {
|
||||
|
||||
// This is private - only created by the kernel
|
||||
Point::Point(Body* _body) :
|
||||
numOwners(1),
|
||||
body(_body ? _body : Body::getWorldBody())
|
||||
|
||||
{}
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
|
||||
void Point::step()
|
||||
{
|
||||
worldPos = body->getCoordinateFrame().pointToWorldSpace(localPos);
|
||||
|
||||
force = Vector3::zero();
|
||||
}
|
||||
|
||||
void Point::forceToBody()
|
||||
{
|
||||
body->accumulateForce(force, worldPos);
|
||||
|
||||
}
|
||||
|
||||
void Point::setLocalPos(const Vector3& _localPos)
|
||||
{
|
||||
localPos = _localPos;
|
||||
worldPos = body->getCoordinateFrame().pointToWorldSpace(localPos);
|
||||
}
|
||||
|
||||
void Point::setWorldPos(const Vector3& _worldPos)
|
||||
{
|
||||
worldPos = _worldPos;
|
||||
localPos = body->getCoordinateFrame().pointToObjectSpace(worldPos);
|
||||
}
|
||||
|
||||
|
||||
} // namespace RBX
|
||||
|
||||
@@ -0,0 +1,220 @@
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/PolyConnectors.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
#include "Util/Math.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
/*
|
||||
Position == position in world coordinates of deepest penetration point.
|
||||
Length == (negative) value - amount of penetration
|
||||
Normal == points "away" from the b0 object, into the b1 object
|
||||
*/
|
||||
|
||||
void BallPlaneConnector::updateContactPoint()
|
||||
{
|
||||
const Vector3& p0 = geoPair.body0->getPosFast();
|
||||
const CoordinateFrame& c1 = geoPair.body1->getCoordinateFrameFast();
|
||||
|
||||
Vector3 p1 = c1.pointToWorldSpace(offset);
|
||||
// contactPoint.normal = -Math::getWorldNormal(normalId1, c1);
|
||||
contactPoint.normal = -c1.vectorToWorldSpace(normal); // points "away from b0"
|
||||
|
||||
Vector3 delta = p1 - p0; // vector from p1 to p0, edge point to center of ball
|
||||
Vector3 centerToPlanePoint = contactPoint.normal * contactPoint.normal.dot(delta);
|
||||
contactPoint.position = p0 + centerToPlanePoint;
|
||||
contactPoint.length = centerToPlanePoint.length() - radius;
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
|
||||
void BallEdgeConnector::updateContactPoint()
|
||||
{
|
||||
const Vector3& p0 = geoPair.body0->getPosFast();
|
||||
const CoordinateFrame& c1 = geoPair.body1->getCoordinateFrameFast();
|
||||
|
||||
Vector3 p1 = c1.pointToWorldSpace(offset);
|
||||
Vector3 edgeNormal = c1.vectorToWorldSpace(normal);
|
||||
Vector3 delta = p1 - p0; // vector from p0 to p1, center of ball to edge point
|
||||
Vector3 projection = edgeNormal * edgeNormal.dot(delta);
|
||||
contactPoint.normal = delta - projection; // right now normal is not unitized
|
||||
contactPoint.position = p0 + contactPoint.normal;
|
||||
contactPoint.length = contactPoint.normal.unitize() - radius;
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
|
||||
void BallVertexConnector::updateContactPoint()
|
||||
{
|
||||
const Vector3& pCenter = geoPair.body0->getPosFast();
|
||||
contactPoint.position = geoPair.body1->getCoordinateFrameFast().pointToWorldSpace(offset);
|
||||
contactPoint.normal = contactPoint.position - pCenter;
|
||||
contactPoint.length = contactPoint.normal.unitize() - radius;
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
//////////////////////////////////////////////////////////////////////////////////////////
|
||||
|
||||
|
||||
void FaceVertexConnector::updateContactPoint()
|
||||
{
|
||||
Plane planeInWorld = geoPair.body0->getCoordinateFrameFast().toWorldSpace(facePlane);
|
||||
|
||||
contactPoint.position = geoPair.body1->getCoordinateFrameFast().pointToWorldSpace(vertexOffset);
|
||||
|
||||
contactPoint.normal = planeInWorld.normal();
|
||||
contactPoint.length = planeInWorld.distance(contactPoint.position);
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
/*
|
||||
|
||||
void FaceEdgeConnector::updateContactPoint()
|
||||
{
|
||||
const CoordinateFrame& c0 = geoPair.body0->getCoordinateFrameFast();
|
||||
const CoordinateFrame& c1 = geoPair.body1->getCoordinateFrameFast();
|
||||
|
||||
Line line0World = c0.toWorldSpace(faceLine);
|
||||
Line line1World = c1.toWorldSpace(edgeLine);
|
||||
|
||||
Vector3 pa = line0World.point();
|
||||
Vector3 pb = line1World.point();
|
||||
|
||||
Vector3 ua = line0World.direction();
|
||||
Vector3 ub = line1World.direction();
|
||||
|
||||
Vector3 p = pb - pa;
|
||||
float uaub = ua.dot(ub);
|
||||
float q1 = ua.dot(p);
|
||||
float q2 = -ub.dot(p);
|
||||
float d = 1 - uaub*uaub;
|
||||
if (d > 1e-6f) {
|
||||
d = 1.0f / d;
|
||||
float alpha = (q1 + uaub*q2)*d;
|
||||
float beta = (uaub*q1 + q2)*d;
|
||||
pa = pa + alpha * ua;
|
||||
pb = pb + beta * ub;
|
||||
|
||||
contactPoint.position = pa;
|
||||
contactPoint.normal = pa - pb; // backwards
|
||||
contactPoint.length = -contactPoint.normal.unitize();
|
||||
|
||||
Vector3 outwards = c0.vectorToWorldSpace(facePlane.normal());
|
||||
|
||||
if (params.normal.dot(outwards) > 0.0) { // lines are overlapping
|
||||
RBXASSERT(params.length <= 0.0);
|
||||
}
|
||||
else {
|
||||
contactPoint.normal = -contactPoint.normal; // not overlapping
|
||||
contactPoint.length = -contactPoint.length;
|
||||
RBXASSERT(contactPoint.length >= 0.0);
|
||||
}
|
||||
}
|
||||
else {
|
||||
contactPoint.position = pa;
|
||||
contactPoint.normal = ua;
|
||||
contactPoint.length = 0.0; // hopefully will blow out;
|
||||
}
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
*/
|
||||
|
||||
|
||||
void FaceEdgeConnector::updateContactPoint()
|
||||
{
|
||||
const CoordinateFrame& c0 = geoPair.body0->getCoordinateFrameFast();
|
||||
const CoordinateFrame& c1 = geoPair.body1->getCoordinateFrameFast();
|
||||
|
||||
Line line0World = c0.toWorldSpace(faceLine);
|
||||
Line line1World = c1.toWorldSpace(edgeLine);
|
||||
|
||||
Vector3 p0, p1;
|
||||
|
||||
if (Line::closestPoints(line0World, line1World, p0, p1))
|
||||
{
|
||||
contactPoint.position = p0;
|
||||
contactPoint.normal = p0 - p1; // backwards
|
||||
contactPoint.length = -contactPoint.normal.unitize(1e-20);
|
||||
|
||||
Vector3 outwards = c0.vectorToWorldSpace(facePlane.normal());
|
||||
|
||||
if (contactPoint.normal.dot(outwards) > 0.0) { // lines are overlapping
|
||||
RBXASSERT(contactPoint.length <= 0.0);
|
||||
}
|
||||
else {
|
||||
contactPoint.normal = -contactPoint.normal; // not overlapping
|
||||
contactPoint.length = -contactPoint.length;
|
||||
RBXASSERT(contactPoint.length >= 0.0);
|
||||
}
|
||||
}
|
||||
else {
|
||||
contactPoint.position = p0;
|
||||
contactPoint.normal = c0.vectorToWorldSpace(facePlane.normal());
|
||||
contactPoint.length = 0.0; // hopefully will blow out;
|
||||
}
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
void EdgeEdgeConnector::updateContactPoint()
|
||||
{
|
||||
const CoordinateFrame& c0 = geoPair.body0->getCoordinateFrameFast();
|
||||
const CoordinateFrame& c1 = geoPair.body1->getCoordinateFrameFast();
|
||||
|
||||
Line line0World = c0.toWorldSpace(edgeLine0);
|
||||
Line line1World = c1.toWorldSpace(edgeLine1);
|
||||
|
||||
Vector3 pa = line0World.point();
|
||||
Vector3 pb = line1World.point();
|
||||
|
||||
Vector3 ua = line0World.direction();
|
||||
Vector3 ub = line1World.direction();
|
||||
|
||||
Vector3 p = pb - pa;
|
||||
float uaub = ua.dot(ub);
|
||||
float q1 = ua.dot(p);
|
||||
float q2 = -ub.dot(p);
|
||||
float d = 1 - uaub*uaub;
|
||||
if (d > 1e-6f) {
|
||||
d = 1.0f / d;
|
||||
float alpha = (q1 + uaub*q2)*d;
|
||||
float beta = (uaub*q1 + q2)*d;
|
||||
pa = pa + alpha * ua;
|
||||
pb = pb + beta * ub;
|
||||
|
||||
// contactPoint.position = 0.5 * (pa + pb);
|
||||
// contactPoint.normal = pa - pb;
|
||||
// contactPoint.length = -params.normal.unitize();
|
||||
|
||||
contactPoint.position = pa;
|
||||
contactPoint.normal = pa - pb; // backwards
|
||||
contactPoint.length = -contactPoint.normal.unitize();
|
||||
|
||||
Vector3 b0ToB1 = c1.translation - c0.translation;
|
||||
if (contactPoint.normal.dot(b0ToB1) > 0.0) { // lines are overlapping
|
||||
RBXASSERT(contactPoint.length <= 0.0);
|
||||
}
|
||||
else {
|
||||
contactPoint.normal = -contactPoint.normal; // not overlapping
|
||||
contactPoint.length = -contactPoint.length;
|
||||
RBXASSERT(contactPoint.length >= 0.0);
|
||||
}
|
||||
}
|
||||
else {
|
||||
contactPoint.position = pa;
|
||||
contactPoint.normal = ua;
|
||||
contactPoint.length = 0.0; // hopefully will blow out;
|
||||
}
|
||||
ContactConnector::updateContactPoint();
|
||||
}
|
||||
|
||||
|
||||
} // namespace
|
||||
@@ -0,0 +1,304 @@
|
||||
/* Copyright 2003-2005 ROBLOX Corporation, All Rights Reserved */
|
||||
#include "stdafx.h"
|
||||
|
||||
#include "V8Kernel/SimBody.h"
|
||||
#include "V8Kernel/Body.h"
|
||||
#include "V8Kernel/Constants.h"
|
||||
#include "Util/Units.h"
|
||||
|
||||
namespace RBX {
|
||||
|
||||
float SimBody::maxTorqueXX = 1e15f;
|
||||
float SimBody::maxForceXX = 1e15f;
|
||||
float SimBody::maxLinearImpulseXX = 1e15f;
|
||||
float SimBody::maxRotationalImpulseXX = 1e15f;
|
||||
|
||||
float SimBody::maxDebugTorque() {return maxTorqueXX;}
|
||||
float SimBody::maxDebugForce() {return maxForceXX;}
|
||||
float SimBody::maxDebugLinearImpulse() {return maxLinearImpulseXX;}
|
||||
float SimBody::maxDebugRotationalImpulse() {return maxRotationalImpulseXX;}
|
||||
|
||||
SimBody::SimBody(Body* body)
|
||||
: body(body)
|
||||
, dt(0.0f)
|
||||
, dirty(true)
|
||||
, symmetricContact(false)
|
||||
, angMomentum(Vector3::zero())
|
||||
, moment(Vector3::zero())
|
||||
, momentRecip(Vector3::zero())
|
||||
, massRecip(0.0f)
|
||||
, constantForceY(0.0f)
|
||||
, force(Vector3::zero())
|
||||
, torque(Vector3::zero())
|
||||
, impulse(Vector3::zero())
|
||||
, impulseLast(Vector3::zero())
|
||||
, rotationalImpulse(Vector3::zero())
|
||||
, penetrationTorque(Vector3::zero())
|
||||
, freeFallBodyIndex(-1)
|
||||
, realTimeBodyIndex(-1)
|
||||
, jointBodyIndex(-1)
|
||||
, buoyancyBodyIndex(-1)
|
||||
, contactBodyIndex(-1)
|
||||
, numOfConnectors(0)
|
||||
, numOfHumanoidConnectors(0)
|
||||
, numOfSecondPassConnectors(0)
|
||||
, numOfRealTimeConnectors(0)
|
||||
, numOfJointConnectors(0)
|
||||
, numOfBuoyancyConnectors(0)
|
||||
, numOfContactConnectors(0)
|
||||
{
|
||||
momentRecipWorld = Matrix3::zero();
|
||||
uid = body->getUID();
|
||||
}
|
||||
|
||||
SimBody::~SimBody()
|
||||
{
|
||||
RBXASSERT(dt == 0.0f);
|
||||
RBXASSERT(freeFallBodyIndex == -1);
|
||||
RBXASSERT(realTimeBodyIndex == -1);
|
||||
RBXASSERT(jointBodyIndex == -1);
|
||||
RBXASSERT(contactBodyIndex == -1);
|
||||
}
|
||||
|
||||
PV SimBody::getOwnerPV()
|
||||
{
|
||||
RBXASSERT(!dirty);
|
||||
Vector3 cofmInBody = body->getBranchCofmOffset();
|
||||
return pv.pvAtLocalOffset(-cofmInBody);
|
||||
}
|
||||
|
||||
void SimBody::update()
|
||||
{
|
||||
RBXASSERT(dirty);
|
||||
|
||||
Vector3 cofmInBody = body->getBranchCofmOffset();
|
||||
pv = body->getPvUnsafe().pvAtLocalOffset(cofmInBody);
|
||||
qOrientation = Quaternion(pv.position.rotation);
|
||||
qOrientation.normalize();
|
||||
|
||||
massRecip = 1.0f / body->getBranchMass();
|
||||
moment = body->getBranchIBodyV3();
|
||||
momentRecip = Vector3(1,1,1) / moment;
|
||||
updateAngMomentum();
|
||||
updateMomentRecipWorld();
|
||||
constantForceY = body->getBranchMass() * Units::kmsAccelerationToRbx( Constants::getKmsGravity() );
|
||||
|
||||
RBXASSERT_VERY_FAST(Math::longestVector3Component(force) < 1e20f);
|
||||
RBXASSERT_VERY_FAST(constantForceY < 1e20f);
|
||||
RBXASSERT_VERY_FAST(body->cofmIsClean());
|
||||
RBXASSERT_FISHING(!Math::isNanInfDenormVector3(qOrientation.imag()));
|
||||
|
||||
RBXASSERT(dirty); // concurrency check
|
||||
|
||||
dirty = false;
|
||||
}
|
||||
|
||||
// iWorldInverse = rot * momentRecip * rot.transpose();
|
||||
|
||||
inline void SimBody::updateMomentRecipWorld()
|
||||
{
|
||||
Matrix3 temp;
|
||||
Math::mulMatrixDiagVector(pv.position.rotation, momentRecip, temp);
|
||||
Math::mulMatrixMatrixTranspose(temp, pv.position.rotation, momentRecipWorld);
|
||||
}
|
||||
|
||||
|
||||
void SimBody::clearVelocity()
|
||||
{
|
||||
pv.velocity.rotational = Vector3::zero();
|
||||
pv.velocity.linear = Vector3::zero();
|
||||
body->pv.velocity = pv.velocity;
|
||||
body->advanceStateIndex();
|
||||
}
|
||||
|
||||
void SimBody::updateAngMomentum()
|
||||
{
|
||||
Matrix3 temp;
|
||||
Matrix3 iWorld;
|
||||
Math::mulMatrixDiagVector(pv.position.rotation, moment, temp);
|
||||
Math::mulMatrixMatrixTranspose(temp, pv.position.rotation, iWorld);
|
||||
angMomentum = iWorld * pv.velocity.rotational;
|
||||
}
|
||||
|
||||
// rotationalVelocity = iWorldInverse * angMomentum;
|
||||
inline Vector3 SimBody::computeRotationVelocityFromMomentumFast()
|
||||
{
|
||||
return momentRecipWorld * angMomentum;
|
||||
}
|
||||
|
||||
// rotationalVelocity = iWorldInverse * angMomentum;
|
||||
inline Vector3 SimBody::computeRotationVelocityFromMomentum()
|
||||
{
|
||||
updateMomentRecipWorld();
|
||||
return computeRotationVelocityFromMomentumFast();
|
||||
}
|
||||
|
||||
static const Vector3 denormalSmall(1e-20f, 1e-20f, 1e-20f);
|
||||
|
||||
void SimBody::step()
|
||||
{
|
||||
RBXASSERT(!dirty);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(force) < 1e19);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.position.translation) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.velocity.linear) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(force) < 1e19);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.position.translation) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.velocity.linear) < 1e6);
|
||||
|
||||
angMomentum *= 0.9998f; // damping
|
||||
angMomentum += (rotationalImpulse + torque * dt);
|
||||
pv.velocity.rotational = computeRotationVelocityFromMomentum() + denormalSmall;
|
||||
Quaternion qDot(Quaternion(pv.velocity.rotational) * qOrientation * 0.5f);
|
||||
qOrientation += qDot * dt;
|
||||
qOrientation.normalize();
|
||||
qOrientation.toRotationMatrix(pv.position.rotation);
|
||||
|
||||
pv.velocity.linear += massRecip * (impulse + force * dt);
|
||||
pv.position.translation += pv.velocity.linear * dt;
|
||||
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.position.translation) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.velocity.linear) < 1e6);
|
||||
|
||||
clearForceAccumulators(); // as opposed to reset - this is the fast version
|
||||
clearImpulseAccumulators(); // as opposed to reset - this is the fast version
|
||||
|
||||
angMomentum += denormalSmall;
|
||||
pv.velocity.linear += denormalSmall;
|
||||
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormVector3(pv.position.translation));
|
||||
RBXASSERT_FISHING(!Math::isNanInfDenormVector3(qOrientation.imag()));
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormMatrix3(pv.position.rotation));
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormVector3(pv.velocity.linear));
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormVector3(pv.velocity.rotational));
|
||||
|
||||
body->pv = (body->cofm == NULL) ? pv : getOwnerPV(); // NUKE THIS WHEN ALL SET
|
||||
body->advanceStateIndex();
|
||||
}
|
||||
|
||||
void SimBody::stepVelocity()
|
||||
{
|
||||
RBXASSERT(!dirty);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(force) < 1e19);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.position.translation) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.velocity.linear) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(force) < 1e19);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.position.translation) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.velocity.linear) < 1e6);
|
||||
|
||||
angMomentum *= 0.99621f; // damping 0.9998 ^ 19
|
||||
angMomentum += rotationalImpulse + torque * dt;
|
||||
pv.velocity.rotational = computeRotationVelocityFromMomentum() + denormalSmall;
|
||||
impulseLast = impulse + force * dt;
|
||||
pv.velocity.linear += massRecip * impulseLast;
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.velocity.linear) < 1e6);
|
||||
|
||||
clearForceAccumulators(); // as opposed to reset - this is the fast version
|
||||
clearImpulseAccumulators(); // as opposed to reset - this is the fast version
|
||||
|
||||
angMomentum += denormalSmall;
|
||||
pv.velocity.linear += denormalSmall;
|
||||
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormVector3(pv.velocity.linear));
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormVector3(pv.velocity.rotational));
|
||||
body->pv.velocity = (body->cofm == NULL) ? pv.velocity : getOwnerPV().velocity; // NUKE THIS WHEN ALL SET
|
||||
body->advanceStateIndex();
|
||||
}
|
||||
|
||||
void SimBody::applyImpulse(const Vector3& _impulse, const Vector3& worldPos)
|
||||
{
|
||||
RBXASSERT(!dirty);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(force) < 1e19);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.position.translation) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.velocity.linear) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(force) < 1e19);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.position.translation) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.velocity.linear) < 1e6);
|
||||
|
||||
Vector3 localPosWorld = worldPos - pv.position.translation;
|
||||
Vector3 _rotationalImpulse = localPosWorld.cross(_impulse);
|
||||
|
||||
angMomentum *= 0.9998f; // damping
|
||||
angMomentum += _rotationalImpulse;
|
||||
pv.velocity.rotational = computeRotationVelocityFromMomentumFast() + denormalSmall;
|
||||
pv.velocity.linear += massRecip * _impulse + denormalSmall;
|
||||
|
||||
angMomentum += denormalSmall;
|
||||
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.velocity.linear) < 1e6);
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormVector3(pv.velocity.linear));
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormVector3(pv.velocity.rotational));
|
||||
|
||||
body->pv.velocity = (body->cofm == NULL) ? pv.velocity : getOwnerPV().velocity; // NUKE THIS WHEN ALL SET
|
||||
body->advanceStateIndex();
|
||||
}
|
||||
|
||||
void SimBody::stepPosition()
|
||||
{
|
||||
RBXASSERT(!dirty);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.position.translation) < 1e6);
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.velocity.linear) < 1e6);
|
||||
|
||||
Quaternion qDot(Quaternion(pv.velocity.rotational) * qOrientation * 0.5f);
|
||||
qOrientation += qDot * dt;
|
||||
qOrientation.normalize();
|
||||
qOrientation.toRotationMatrix(pv.position.rotation);
|
||||
|
||||
pv.position.translation += pv.velocity.linear * dt;
|
||||
|
||||
RBXASSERT_SLOW(Math::longestVector3Component(pv.position.translation) < 1e6);
|
||||
RBXASSERT(!hasExternalForceOrImpulse());
|
||||
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormVector3(pv.position.translation));
|
||||
RBXASSERT_FISHING(!Math::isNanInfDenormVector3(qOrientation.imag()));
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormMatrix3(pv.position.rotation));
|
||||
body->pv = (body->cofm == NULL) ? pv : getOwnerPV(); // NUKE THIS WHEN ALL SET
|
||||
body->advanceStateIndex();
|
||||
}
|
||||
|
||||
|
||||
// Optimized free fall integrator that doesn't do rotational velocity update
|
||||
void SimBody::stepFreeFall()
|
||||
{
|
||||
RBXASSERT(!hasExternalForceOrImpulse());
|
||||
pv.velocity.rotational *= 0.99621f; /* 0.9998 ^ 19 */
|
||||
Quaternion qDot(Quaternion(pv.velocity.rotational) * qOrientation * 0.5f);
|
||||
qOrientation += qDot * dt;
|
||||
qOrientation.normalize();
|
||||
qOrientation.toRotationMatrix(pv.position.rotation);
|
||||
RBXASSERT_VERY_FAST(!Math::isNanInfDenormMatrix3(pv.position.rotation));
|
||||
|
||||
// Assumption: Gravity acceleration remains constant in the current engine.
|
||||
// In the future if gravity acceleration can change we need to use the actual gravity force here
|
||||
//
|
||||
float dyHalf = Units::kmsAccelerationToRbx( Constants::getKmsGravity() ) * dt / 2.0f;
|
||||
pv.velocity.linear.y += dyHalf;
|
||||
pv.position.translation += pv.velocity.linear * dt;
|
||||
pv.velocity.linear.y += dyHalf;
|
||||
|
||||
pv.velocity.rotational += denormalSmall;
|
||||
pv.velocity.linear += denormalSmall;
|
||||
|
||||
body->pv = (body->cofm == NULL) ? pv : getOwnerPV(); // NUKE THIS WHEN ALL SET
|
||||
body->advanceStateIndex();
|
||||
}
|
||||
|
||||
void SimBody::updateFromSolver( const Vector3& newPosition, const Matrix3& newOrientation,
|
||||
const Vector3& newLinearVelocity, const Vector3& newAngularVelocity )
|
||||
{
|
||||
pv.position.translation = newPosition;
|
||||
pv.position.rotation = newOrientation;
|
||||
pv.velocity.linear = newLinearVelocity;
|
||||
pv.velocity.rotational = newAngularVelocity;
|
||||
qOrientation = Quaternion(pv.position.rotation);
|
||||
|
||||
clearForceAccumulators();
|
||||
clearImpulseAccumulators();
|
||||
|
||||
body->pv = (body->cofm == NULL) ? pv : getOwnerPV();
|
||||
body->advanceStateIndex();
|
||||
|
||||
dirty = true;
|
||||
update();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
Reference in New Issue
Block a user