//--------------------------------------------------------------
void ofxBulletRigidBody::createRigidBody(int mass, const btTransform startTrans) {
btVector3 inertia(0, 0, 0);
shape->calculateLocalInertia(mass, inertia);
btMotionState* myMotionState;
myMotionState = new btDefaultMotionState(startTrans);
/*switch (bodyType) {
case 0:
myMotionState = new btDefaultMotionState(startTrans);
mass = 0;
break;
case 1:
myMotionState = new btDefaultMotionState(startTrans);
break;
case 2:
myMotionState = new MyKinematicMotionState(startTrans);
break;
default:
myMotionState = new btDefaultMotionState(startTrans);
break;
}
*/
btRigidBody::btRigidBodyConstructionInfo rbci(mass, myMotionState, shape, inertia);
rbci.m_startWorldTransform = startTrans;
body = new btRigidBody(rbci);
//if (bodyType == 2) {
// psb->setCollisionFlags(psb->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
// psb->setActivationState(DISABLE_DEACTIVATION);
//}
}
void CylinderCollider::Awake() {
auto go = GetGameObject();
auto& trans = go->GetLocalTransform();
auto& btTrans = trans.GetBtTransform();
float halfX = m_halfExtents.x;
float halfY = m_halfExtents.y;
float halfZ = m_halfExtents.z;
halfX *= trans.Scale.x;
halfY *= trans.Scale.y;
halfZ *= trans.Scale.z;
m_collisionShape = new btCylinderShape(btVector3(halfX, halfY, halfZ));
btVector3 inertia;
m_collisionShape->calculateLocalInertia(Mass, inertia);
m_defaultMotionState = new btDefaultMotionState(btTrans);
btRigidBody::btRigidBodyConstructionInfo rbci(Mass, m_defaultMotionState, m_collisionShape, inertia);
m_rigidBody = new btRigidBody(rbci);
m_world->AddCollider(this);
}
class btRigidBody* MyMultiBodyCreator::allocateRigidBody(int urdfLinkIndex, btScalar mass, const btVector3& localInertiaDiagonal, const btTransform& initialWorldTrans, class btCollisionShape* colShape)
{
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0, colShape, localInertiaDiagonal);
rbci.m_startWorldTransform = initialWorldTrans;
btRigidBody* body = new btRigidBody(rbci);
return body;
}
btRigidBody* create_rigid_body(float mass, Node* node,
btCollisionShape* shape) {
btMotionState* motion_state = new NodeMotionState(node);
btVector3 inertia(0, 0, 0);
shape->calculateLocalInertia(mass, inertia);
btRigidBody::btRigidBodyConstructionInfo rbci(mass, motion_state, shape,
inertia);
return new btRigidBody(rbci);
}
btRigidBody* BasicDemo::localCreateRigidBody(btScalar mass,const btTransform& startTrans,btCollisionShape* colShape)
{
btVector3 inertia(0,0,0);
if (mass)
colShape->calculateLocalInertia(mass,inertia);
btRigidBody::btRigidBodyConstructionInfo rbci(mass,0,colShape,inertia);
rbci.m_startWorldTransform = startTrans;
btRigidBody* body = new btRigidBody(rbci);
m_dynamicsWorld->addRigidBody(body);
return body;
}
plRigidBodyHandle plCreateRigidBody( void* user_data, float mass, plCollisionShapeHandle cshape )
{
btTransform trans;
trans.setIdentity();
btVector3 localInertia(0,0,0);
btCollisionShape* shape = reinterpret_cast<btCollisionShape*>( cshape);
btAssert(shape);
if (mass)
{
shape->calculateLocalInertia(mass,localInertia);
}
void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
btRigidBody* body = new (mem)btRigidBody(rbci);
body->setWorldTransform(trans);
body->setUserPointer(user_data);
return (plRigidBodyHandle) body;
}
PlRigidBody* pl_rigidbody_new(void* user_data, float mass, PlCollisionShape* cshape )
{
btTransform trans;
trans.setIdentity();
btVector3 localInertia(0,0,0);
CAST_ASSERT(cshape,btCollisionShape*,shape);
if (mass)
{
shape->calculateLocalInertia(mass,localInertia);
}
void* mem = btAlignedAlloc(sizeof(btRigidBody),16);
btRigidBody::btRigidBodyConstructionInfo rbci(mass, 0,shape,localInertia);
btRigidBody* body = new (mem)btRigidBody(rbci);
body->setWorldTransform(trans);
body->setUserPointer(user_data);
return (PlRigidBody*) body;
}
void Physics::makeLine() {
Segment* s = new Segment;
//cout << "Making a line" << endl;
s->dis = v_cur - v_prev;
s->max = v_cur;
s->min = v_prev;
s->cs = new btCylinderShapeZ(btVector3(1.0f, 0.0f, s->dis.length() - s->dis.length()/10));
s->cs->setMargin(0.01f);
//s.cs = new btCylinderShape(btVector3(0.5f, 0.5f, s.dis.length()));
//btBoxShape* wire = new btBoxShape(btVector3(0.05f, dis.length(), 0.05f));
btScalar mass = 0.0;//(btScalar)0.5;
//btScalar mass = 0.5;
btVector3 inertia(0, 0, 0);
s->cs->calculateLocalInertia(mass, inertia);
btVector3 y = btVector3(0.0f, 0.0f, 1.0f);
//btVector3 y = btVector3(0.0f, 0.0f, 1.0f);
btVector3 v = (y.cross(s->dis));
float w = sqrt((y.length() * y.length()) * (s->dis.length() * s->dis.length())) + y.dot(s->dis);
if(w == 0) {
v.setY(1);
}
s->rot = btQuaternion(v.getX(), v.getY(), v.getZ(), w);
if(w!=0) s->rot.normalize();
//cout << s.rot.getX() << " " << s.rot.getY() << " " << s.rot.getZ() << " " << s.rot.getW() << endl;
s->frame = btTransform(s->rot, v_prev);
btDefaultMotionState* ms = new btDefaultMotionState(s->frame);// + (0.5)*s.dis));
btRigidBody::btRigidBodyConstructionInfo rbci(mass, ms, s->cs, inertia);
s->rb = new btRigidBody(rbci);
s->rb->setActivationState(DISABLE_DEACTIVATION);
ddw->addRigidBody(s->rb);
//s.rb->setGravity(btVector3(0,0,0));
//s.rb->setLinearVelocity(btVector3(0,0,0));
if(s->dis.length() > 0) {
wires.push_back(s);
cout << "lollol" << endl;
}
if(prev_segment == nullptr) {
//cout << "Nope" << endl;
}
else {
//cout << "Constraining" << endl;
//prev_segment->rb->setLinearVelocity(btVector3(0,0,0));
//btTransform fia = btTransform(btQuaternion(), btVector3(0.0, 0.0, 0.0));
//btTransform fib = btTransform(btQuaternion(), btVector3(0.0, s.dis.length(), 0.0));
//btTransform fia = btTransform(s.rot, btVector3(0.0, 0.0, 0.0));
//btTransform fib = btTransform(prev_segment->rot, btVector3(0.0, s.dis.length()-.25, 0.0));
//btTransform fia = btTransform(s.rot, v_cur);
//btTransform fib = btTransform(prev_segment->rot, v_prev);
/*
btVector3 v = v_prev.cross(v_cur);
float wx = sqrt((v_cur.length() * v_cur.length()) * (v_prev.length() * v_prev.length())) + v_cur.dot(v_prev);
if(wx < 0) v.setY(1);
btQuaternion q = btQuaternion(v.getX(), v.getY(), v.getZ(), wx);
if(wx != 0) q.normalize();
*/
btTransform fia = btTransform::getIdentity();
fia.setIdentity();
fia.setOrigin(btVector3(0,0,0));
//btTransform fib = btTransform(q, btVector3(0,prev_segment->dis.length(),0));
//btTransform fib = prev_segment->rb->getCenterOfMassTransform().inverse() * s.rb->getCenterOfMassTransform();
btTransform fib = btTransform::getIdentity();
fib.setIdentity();
fib.setOrigin(btVector3(0,0,prev_segment->dis.length()));
//cons.push_back(new btFixedConstraint(*s.rb, *prev_segment->rb, fia, fib));
//btFixedConstraint* dof = new btFixedConstraint(*s.rb, *prev_segment->rb, fia, fib);
btGeneric6DofConstraint* dof = new btGeneric6DofConstraint(*s->rb, *prev_segment->rb, fia, fib, true);
//btGeneric6DofConstraint* dof = new btGeneric6DofConstraint(*s.rb, *prev_segment->rb, btTransform::getIdentity(), btTransform::getIdentity(), true);
cout<< "hiiiii" << endl;
dof->setAngularLowerLimit(btVector3(0, 0, 0));
dof->setAngularUpperLimit(btVector3(0, 0, 0));
dof->setLinearLowerLimit(btVector3(0, 0, 0));
dof->setLinearUpperLimit(btVector3(0, 0, 0));
if (prev_segment->dis.length() > 0)
cons.push_back(dof);
//ddw->addConstraint(dof, true);
}
prev_segment = s;
v_prev = v_cur;
//wires.push_back(new btRigidBody(wirerbci));
}
//--------------------------------------------------------------
void ofxBulletRigidBody::create(btDynamicsWorld * world) {
if(world == NULL) return;
this->world = world;
float x = btScalar(ofRandom(-1, 1));
float y = btScalar(ofRandom(-1, -6));
float z = btScalar(ofRandom(-3, 3));
btTransform startTransform;
startTransform.setIdentity();
startTransform.setOrigin(btVector3(x, y, z));
float radius = 1;
shape = new btSphereShape(radius);
btVector3 inertia(0, 0, 0);
float mass = 3.0;
shape->calculateLocalInertia(mass, inertia);
btMotionState * myMotionState;
myMotionState = new btDefaultMotionState(startTransform);
/*
struct btRigidBodyConstructionInfo
{
btScalar m_mass;
///When a motionState is provided, the rigid body will initialize its world transform from the motion state
///In this case, m_startWorldTransform is ignored.
btMotionState* m_motionState;
btTransform m_startWorldTransform;
btCollisionShape* m_collisionShape;
btVector3 m_localInertia;
btScalar m_linearDamping;
btScalar m_angularDamping;
///best simulation results when friction is non-zero
btScalar m_friction;
///best simulation results using zero restitution.
btScalar m_restitution;
btScalar m_linearSleepingThreshold;
btScalar m_angularSleepingThreshold;
//Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
//Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
bool m_additionalDamping;
btScalar m_additionalDampingFactor;
btScalar m_additionalLinearDampingThresholdSqr;
btScalar m_additionalAngularDampingThresholdSqr;
btScalar m_additionalAngularDampingFactor;
btRigidBodyConstructionInfo( btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
m_mass(mass),
m_motionState(motionState),
m_collisionShape(collisionShape),
m_localInertia(localInertia),
m_linearDamping(btScalar(0.)),
m_angularDamping(btScalar(0.)),
m_friction(btScalar(0.5)),
m_restitution(btScalar(0.)),
m_linearSleepingThreshold(btScalar(0.8)),
m_angularSleepingThreshold(btScalar(1.f)),
m_additionalDamping(false),
m_additionalDampingFactor(btScalar(0.005)),
m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
m_additionalAngularDampingFactor(btScalar(0.01))
{
m_startWorldTransform.setIdentity();
}
};
*/
btRigidBody::btRigidBodyConstructionInfo rbci(mass, myMotionState, shape, inertia);
rbci.m_friction = 0.7;
rbci.m_restitution = 0.07;
rbci.m_startWorldTransform = startTransform;
body = new btRigidBody(rbci);
// now add it to the world
world->addRigidBody(body);
}
