void btRigidBody::calculateLocalEnergy(){
float modInertia = m_invInertiaLocal.length();
float modAngVel = m_angularVelocity.length();
float modLinVel = m_linearVelocity.length();
m_localEnergy = 0.5f*1/getInvMass()*modLinVel*modLinVel + 0.5f*modInertia*modAngVel*modAngVel;
}
CsRigidBody* CsRigidBody::Clone() {
btScalar cloneMass = btScalar(1.0) / getInvMass();
btVector3 cloneInertia(btScalar(1.0)/getInvInertiaDiagLocal().x(), btScalar(1.0)/getInvInertiaDiagLocal().y(), btScalar(1.0) / getInvInertiaDiagLocal().z());
CsMotionState *motionState = NULL;
if (getMotionState()) {
CsMotionState *oldState = (CsMotionState*) getMotionState();
motionState = new CsMotionState(*oldState);
// set this from outer scope
motionState->mObj = NULL;
}
CsRigidBody *cloneBody = new CsRigidBody(cloneMass, motionState, getCollisionShape(), cloneInertia);
return cloneBody;
// see test CCDphysics project
}
void PeerCircle::update()
{
// attraction to torrent position
Vec2f dir = mAttractorPos - mPos;
float distSqrd = dir.lengthSquared();
float strength = 5.f;
if ( distSqrd > 0.f )
{
float minDistSqrd = 1000.f;
if ( distSqrd < minDistSqrd )
{
strength = lmap< float >( 1.f, minDistSqrd, 0.f, strength, distSqrd );
strength = math< float >::clamp( strength, 0.f, 5.f );
}
float force = strength * mMass / distSqrd;
Vec2f deltaForce = dir * force;
moveBy( deltaForce * mInvMass, false );
}
// repel other peers
const std::vector< PeerRef > &peers = mTorrentRef->getPeers();
for ( PeerRef peer : peers )
{
if ( peer->getId() == mId )
continue;
auto other = std::static_pointer_cast< PeerCircle >( peer );
Vec2d dir = mPos - other->getPos();
float distSqrd = dir.lengthSquared();
if ( distSqrd > .0f )
{
float force = .1f * mMass * other->getMass() / distSqrd;
Vec2f deltaForce = dir * force;
moveBy( deltaForce * mInvMass, false );
other->moveBy( - deltaForce * other->getInvMass(), false );
}
}
// do verlet
Vec2f curPos = mPos;
Vec2f vel = mPos - mOldPos;
mPos += vel * mDrag;
mOldPos = curPos;
/*
// TODO: attract actual torrent position
Vec2d dir = Vec2d( 500, 500 ) - mPos;
double distSqrd = dir.lengthSquared();
if ( distSqrd > 0. )
{
double f = math< double >::sqrt( distSqrd ) * .00001;
mAcc += dir.normalized() * f;
}
mVel += mAcc;
mPos += mVel;
mVel *= mDecay;
mAcc.set( 0., 0. );
//mRadius = lerp< double, double >( mRadius, mRadiusOrig, .01 );
*/
}
void Body::applyImpulse(sf::Vector2f mImpulse) { velocity += getInvMass() * mImpulse; }