inline btTransform toBt(const mat4x4& a)
{
return btTransform(toBt(submat<float, 3, 3>(a)), btVector3(a(0, 3), a(1, 3), a(2, 3)));
}
void Simulation::Step(float dt) {
// Update the position of the target
if (vis_sim_) {
btTransform light_pos;
light_pos.setIdentity();
light_pos.setOrigin(btVector3(
SettingsManager::Instance()->GetTargetPos().x,
SettingsManager::Instance()->GetTargetPos().y,
SettingsManager::Instance()->GetTargetPos().z));
light_rigid_body_->setCenterOfMassTransform(light_pos);
}
/*
Step through all BulletCreatures and Creatures to update motors
and feed Creature with performance data.
*/
for (int i = 0; i < bt_population_.size(); ++i) {
std::vector<float> input;
input.push_back(1.0);
btQuaternion orientation = bt_population_[i]->GetHead()->getOrientation();
btTransform orientation_matrix = btTransform(orientation);
btTransform inverse_orient = orientation_matrix.inverse();
//light direction
btVector3 head_light_vec = light_rigid_body_->getCenterOfMassPosition() - bt_population_[i]->GetHeadPosition();
btVector3 light_dir = head_light_vec.normalized();
float distance2_to_light = head_light_vec.length2();
//light direction relatie to head
light_dir = inverse_orient*light_dir;
input.push_back(light_dir.getX());
input.push_back(light_dir.getY());
input.push_back(light_dir.getZ());
std::vector<btHingeConstraint*> joints = bt_population_[i]->GetJoints();
//joint angles
for(int j=0; j < joints.size(); j++) {
input.push_back(joints[j]->getHingeAngle());
}
/*
std::vector<btRigidBody*> bodies = bt_population_[i]->GetRigidBodies();
//body velocities
for(int j=0; j < bodies.size(); j++) {
btVector3 vel = inverse_orient*bodies[j]->getAngularVelocity();
vel *= 0.1;
input.push_back(vel.getX());
input.push_back(vel.getY());
input.push_back(vel.getZ());
}*/
bt_population_[i]->UpdateMotors(input);
//std::vector<float> sim_data;
//sim_data.push_back(distance2_to_light);
bt_population_[i]->SetDistanceToLight(distance2_to_light);
bt_population_[i]->CollectData();
}
dynamics_world_->stepSimulation(dt, 1);
counter_ += dt;
}
void BulletPhysicsEngine::update()
{
for (U32 i = 0; i < 1024; ++i)
{
BulletPhysicsObject* obj = &mPhysicsObjects[i];
if ( obj->deleted )
continue;
if ( obj->shouldBeDeleted )
{
if ( obj->initialized )
{
mDynamicsWorld->removeRigidBody(obj->_rigidBody);
obj->destroy();
}
obj->deleted = true;
continue;
}
}
for (U32 i = 0; i < 1024; ++i)
{
BulletPhysicsObject* obj = &mPhysicsObjects[i];
if ( obj->deleted )
continue;
if ( !obj->initialized )
{
obj->initialize();
mDynamicsWorld->addRigidBody(obj->_rigidBody);
} else {
// Pull updates from Physics thread.
btMotionState* objMotion = obj->_rigidBody->getMotionState();
if ( objMotion )
{
btTransform trans;
objMotion->getWorldTransform(trans);
F32 mat[16];
trans.getOpenGLMatrix(mat);
obj->mPosition.set(mat[12], mat[13], mat[14]);
btQuaternion rot = trans.getRotation();
obj->mRotation.set(QuatToEuler(rot.x(), rot.y(), rot.z(), rot.w()));
}
// Apply actions from Game thread.
while ( obj->mPhysicsActions.size() > 0 )
{
Physics::PhysicsAction action = obj->mPhysicsActions.front();
switch(action.actionType)
{
case Physics::PhysicsAction::setPosition:
obj->mPosition = action.vector3Value;
obj->_rigidBody->setWorldTransform(btTransform(btQuaternion(0, 0, 0, 1),btVector3(action.vector3Value.x, action.vector3Value.y, action.vector3Value.z)));
obj->_rigidBody->activate();
break;
case Physics::PhysicsAction::setLinearVelocity:
obj->_rigidBody->setLinearVelocity(btVector3(action.vector3Value.x * 25.0f, action.vector3Value.y * 25.0f, action.vector3Value.z * 25.0f));
obj->_rigidBody->activate();
break;
}
obj->mPhysicsActions.pop_front();
}
}
}
}
//constructor
TransformSender(double x, double y, double z, double yaw, double pitch, double roll, ros::Time time, const std::string& frame_id, const std::string& parent_id) :
ros::node("transform_sender", ros::node::ANONYMOUS_NAME),broadcaster(*this),
transform_(btTransform(btQuaternion(yaw,pitch,roll), btVector3(x,y,z)), time, frame_id , parent_id){};
// callback function for imu data
void OdomEstimationNode::imuCallback(const ImuConstPtr& imu)
{
imu_callback_counter_++;
assert(imu_used_);
// receive data
boost::mutex::scoped_lock lock(imu_mutex_);
imu_stamp_ = imu->header.stamp;
btQuaternion orientation;
quaternionMsgToTF(imu->orientation, orientation);
imu_meas_ = btTransform(orientation, btVector3(0,0,0));
for (unsigned int i=0; i<3; i++)
for (unsigned int j=0; j<3; j++)
imu_covariance_(i+1, j+1) = imu->orientation_covariance[3*i+j];
// Transforms imu data to base_footprint frame
if (!robot_state_.waitForTransform("base_footprint", imu->header.frame_id, imu_stamp_, ros::Duration(0.5))){
// warn when imu was already activated, not when imu is not active yet
if (imu_active_)
ROS_ERROR("Could not transform imu message from %s to base_footprint", imu->header.frame_id.c_str());
else if (my_filter_.isInitialized())
ROS_WARN("Could not transform imu message from %s to base_footprint. Imu will not be activated yet.", imu->header.frame_id.c_str());
else
ROS_DEBUG("Could not transform imu message from %s to base_footprint. Imu will not be activated yet.", imu->header.frame_id.c_str());
return;
}
StampedTransform base_imu_offset;
robot_state_.lookupTransform("base_footprint", imu->header.frame_id, imu_stamp_, base_imu_offset);
imu_meas_ = imu_meas_ * base_imu_offset;
imu_time_ = Time::now();
// manually set covariance untile imu sends covariance
if (imu_covariance_(1,1) == 0.0){
SymmetricMatrix measNoiseImu_Cov(3); measNoiseImu_Cov = 0;
measNoiseImu_Cov(1,1) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(2,2) = pow(0.00017,2); // = 0.01 degrees / sec
measNoiseImu_Cov(3,3) = pow(0.00017,2); // = 0.01 degrees / sec
imu_covariance_ = measNoiseImu_Cov;
}
my_filter_.addMeasurement(StampedTransform(imu_meas_.inverse(), imu_stamp_, "base_footprint", "imu"), imu_covariance_);
// activate imu
if (!imu_active_) {
if (!imu_initializing_){
imu_initializing_ = true;
imu_init_stamp_ = imu_stamp_;
ROS_INFO("Initializing Imu sensor");
}
if ( filter_stamp_ >= imu_init_stamp_){
imu_active_ = true;
imu_initializing_ = false;
ROS_INFO("Imu sensor activated");
}
else ROS_DEBUG("Waiting to activate IMU, because IMU measurements are still %f sec in the future.",
(imu_init_stamp_ - filter_stamp_).toSec());
}
#ifdef __EKF_DEBUG_FILE__
// write to file
double tmp, yaw;
imu_meas_.getBasis().getEulerZYX(yaw, tmp, tmp);
imu_file_ << yaw << endl;
#endif
};
void Chunk::generatePhysics()
{
if( mPhysicsBody != NULL )
{
auto p = mPhysicsBody;
mPhysicsBody = nullptr;
CoreSingleton->physicsMutex.lock();
CoreSingleton->getPhysicsWorld()
->removeRigidBody( p );
CoreSingleton->physicsMutex.unlock();
delete mPhysicsState;
delete mPhysicsBody;
delete mPhysicsShape;
delete mPhysicsMesh;
}
if( mGeometry->vertexCount > 0 && mGeometry->edgeData > 0 )
{
if( getBlockCount() < CHUNK_SIZE )
{
btTriangleIndexVertexArray* meshInterface = new btTriangleIndexVertexArray;
btIndexedMesh part;
int vertSize = sizeof( TerrainVertex );
int indexSize = sizeof( GLedge );
part.m_vertexBase = (const unsigned char*)&mGeometry->vertexData[0].x;
part.m_vertexStride = vertSize;
part.m_numVertices = mGeometry->vertexCount;
part.m_vertexType = PHY_FLOAT;
part.m_triangleIndexBase = (const unsigned char*)&mGeometry->edgeData[0];
part.m_triangleIndexStride = indexSize * 3;
part.m_numTriangles = mGeometry->edgeCount / 3;
part.m_indexType = PHY_SHORT;
meshInterface->addIndexedMesh( part, PHY_SHORT );
//mPhysicsShape = new btBvhTriangleMeshShape( meshInterface, true );//new btConvexTriangleMeshShape( meshInterface );////new btBoxShape( btVector3(8, 64, 8) );
mPhysicsShape = new btBvhTriangleMeshShape( meshInterface, false );
mPhysicsState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3( getX() * CHUNK_WIDTH, getY() * CHUNK_HEIGHT, getZ() * CHUNK_WIDTH)));
btRigidBody::btRigidBodyConstructionInfo ci( 0, mPhysicsState, mPhysicsShape, btVector3(0,0,0) );
mPhysicsBody = new btRigidBody( ci );
mPhysicsBody->setCollisionFlags( mPhysicsBody->getCollisionFlags() | btRigidBody::CF_STATIC_OBJECT );
}
else
{
// Chunk is a solid block.
mPhysicsShape = new btBoxShape( btVector3(CHUNK_WIDTH/2, CHUNK_HEIGHT/2, CHUNK_WIDTH/2) );
mPhysicsState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3( getX() * CHUNK_WIDTH, getY() * CHUNK_HEIGHT, getZ() * CHUNK_WIDTH)));
btRigidBody::btRigidBodyConstructionInfo ci( 0, mPhysicsState, mPhysicsShape, btVector3(0,0,0) );
mPhysicsBody = new btRigidBody( ci );
mPhysicsBody->setCollisionFlags( mPhysicsBody->getCollisionFlags() | btRigidBody::CF_STATIC_OBJECT );
}
CoreSingleton->physicsMutex.lock();
CoreSingleton->getPhysicsWorld()
->addRigidBody( mPhysicsBody );
CoreSingleton->physicsMutex.unlock();
}
}
InverseKinematicsReportPtr
KinematicSolver::internalInverseSoln(const btTransform& pose, Arm* arm,const InverseKinematicsOptions& options) const {
TRACER_ENTER_SCOPE("KinematicSolver::internalInverseSoln(arm@%p)",arm);
InverseKinematicsReportPtr report(new InverseKinematicsReport());
Arm::IdType armId_new = arm_->id();
int armId = armIdFromSerial(arm_->id());
// desired tip position
// btVector3 currentPoint = btVector3(mech->pos.x,mech->pos.y,mech->pos.z) / MICRON_PER_M;
// btVector3 actualPoint = btVector3(pos_d->x,pos_d->y,pos_d->z) / MICRON_PER_M;
// btMatrix3x3 actualOrientation = toBt(ori_d->R);
// btTransform actualPose(actualOrientation,actualPoint);
// btTransform actualPose_fk = pose;
// tb_angles currentPoseAngles = tb_angles(mech->ori.R);
// tb_angles actualPoseAngles = tb_angles(ori_d->R);
//float grasp = GRASP_TO_IK(armId,mech->ori_d.grasp);
float grasp = arm->getJointById(Joint::IdType::GRASP_)->position();
// if (print) {
// log_msg("j s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f y % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f",
// arm->getJointById(Joint::Type::SHOULDER_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::ELBOW_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::TOOL_ROT_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::INSERTION__)->position(),
// arm->getJointById(Joint::Type::WRIST_)->position() RAD2DEG,
// fix_angle(arm->getJointById(Joint::Type::GRIPPER1_)->position() - arm->getJointById(Joint::Type::GRIPPER2_)->position()) / 2 RAD2DEG,
// (arm->getJointById(Joint::Type::GRIPPER1_)->position() + arm->getJointById(Joint::Type::GRIPPER2_)->position()) RAD2DEG,
// arm->getJointById(Joint::Type::GRIPPER1_)->position() RAD2DEG,arm->getJointById(Joint::Type::GRIPPER2_)->position() RAD2DEG);
// log_msg("v s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f y % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f",
// arm->getJointById(Joint::Type::SHOULDER].jvel RAD2DEG,
// arm->getJointById(Joint::Type::ELBOW].jvel RAD2DEG,
// arm->getJointById(Joint::Type::TOOL_ROT].jvel RAD2DEG,
// arm->getJointById(Joint::Type::INSERTION_].jvel,
// arm->getJointById(Joint::Type::WRIST].jvel RAD2DEG,
// (arm->getJointById(Joint::Type::GRASP1].jvel - arm->getJointById(Joint::Type::GRASP2].jvel) / 2 RAD2DEG,
// arm->getJointById(Joint::Type::GRASP1].jvel + arm->getJointById(Joint::Type::GRASP2].jvel RAD2DEG,
// arm->getJointById(Joint::Type::GRASP1].jvel RAD2DEG,arm->getJointById(Joint::Type::GRASP2].jvel RAD2DEG);
// log_msg("t s % 1.3f e % 1.3f r % 1.3f i % 1.3f p % 1.3f g1 % 1.3f g2 % 1.3f",arm->getJointById(Joint::Type::SHOULDER].tau_d,
// arm->getJointById(Joint::Type::ELBOW].tau_d,arm->getJointById(Joint::Type::TOOL_ROT].tau_d,arm->getJointById(Joint::Type::INSERTION_].tau_d,arm->getJointById(Joint::Type::WRIST].tau_d,
// arm->getJointById(Joint::Type::GRASP1].tau_d,arm->getJointById(Joint::Type::GRASP2].tau_d);
// log_msg("d s %d e %d r %d i %d p %d g1 %d g2 %d",tToDACVal(&(arm->getJointById(Joint::Type::SHOULDER])),
// tToDACVal(&(arm->getJointById(Joint::Type::ELBOW])),tToDACVal(&(arm->getJointById(Joint::Type::TOOL_ROT])),tToDACVal(&(arm->getJointById(Joint::Type::INSERTION_])),tToDACVal(&(arm->getJointById(Joint::Type::WRIST])),
// tToDACVal(&(arm->getJointById(Joint::Type::GRASP1])),tToDACVal(&(arm->getJointById(Joint::Type::GRASP2])));
// log_msg("cp (% 1.3f,% 1.3f,% 1.3f\typr (% 1.3f,% 1.3f,% 1.3f))",
// currentPoint.x(),currentPoint.y(),currentPoint.z(),
// currentPoseAngles.yaw_deg,currentPoseAngles.pitch_deg,currentPoseAngles.roll_deg);
// log_msg("pt (% 1.3f,% 1.3f,% 1.3f)\typr (% 1.3f,% 1.3f,% 1.3f)\tg % 1.3f",
// actualPoint.x(),actualPoint.y(),actualPoint.z(),
// actualPoseAngles.yaw_deg,actualPoseAngles.pitch_deg,actualPoseAngles.roll_deg,
// grasp);
//
// btVector3 point = actualPose_fk.getOrigin();
// tb_angles angles = tb_angles(actualPose_fk.getBasis());
// log_msg("fp (% 1.3f,% 1.3f,% 1.3f)\typr (% 2.1f,% 2.1f,% 2.1f)\tg % 1.3f",
// point.x(),point.y(),point.z(),
// angles.yaw_deg,angles.pitch_deg,angles.roll_deg,
// grasp);
//
// }
/*
* Actual pose is in the actual world frame, so we have <actual_world to gripper>
* The ik world frame is the base frame.
* Therefore, we need <base to actual_world> to get <base to gripper>.
* <base to actual_world> is inverse of <actual_world to base>
*
* Since the ik is based on the yaw frame (to which the gripper is fixed), we
* take the pose of the yaw frame, not the gripper frame
*/
btTransform ik_pose = ik_world_to_actual_world(armId) * pose * Tg.inverse();
btMatrix3x3 ik_orientation = ik_pose.getBasis();
btVector3 ik_point = ik_pose.getOrigin();
tb_angles ikPoseAngles = tb_angles(ik_pose);
// if (print) {
// log_msg("ik (%0.4f,%0.4f,%0.4f)\typr (%0.4f,%0.4f,%0.4f)",
// ik_point.x(),ik_point.y(),ik_point.z(),
// ikPoseAngles.yaw_deg,ikPoseAngles.pitch_deg,ikPoseAngles.roll_deg);
// }
float ths_offset, thr_offset, thp_offset;
if (arm->isGold()) {
ths_offset = SHOULDER_OFFSET_GOLD;
thr_offset = TOOL_ROT_OFFSET_GOLD;
thp_offset = WRIST_OFFSET_GOLD;
} else {
ths_offset = SHOULDER_OFFSET_GREEN;
thr_offset = TOOL_ROT_OFFSET_GREEN;
thp_offset = WRIST_OFFSET_GREEN;
}
const float th12 = THETA_12;
const float th23 = THETA_23;
const float ks12 = sin(th12);
const float kc12 = cos(th12);
const float ks23 = sin(th23);
const float kc23 = cos(th23);
const float dw = DW;
btTransform Tworld_to_gripper = ik_pose;
btTransform Tgripper_to_world = ik_pose.inverse();
// if (print) {
// log_msg("Tw2g: %d",PRINT_EVERY);
// for (int i=0;i<3;i++) {
// log_msg(" %0.4f\t%0.4f\t%0.4f\t%0.4f",ik_pose.getBasis()[i][0],ik_pose.getBasis()[i][1],ik_pose.getBasis()[i][2],ik_pose.getOrigin()[i]);
// }
// }
btVector3 origin_in_gripper_frame = Tgripper_to_world.getOrigin();
float px = origin_in_gripper_frame.x();
float py = origin_in_gripper_frame.y();
float pz = origin_in_gripper_frame.z();
float thy = atan2f(py,-px);
float thp;
if (fabs(thy) < 0.001) {
thp = atan2f(-pz, -px/cos(thy) - dw);
// if (print) { log_msg("zero thy: %0.4f, (%0.4f, %0.4f, %0.4f)",thp,px,py,pz); }
} else {
thp = atan2f(-pz, py/sin(thy) - dw);
}
float d = -pz / sin(thp);
float d_act, thp_act, thy_act, g1_act, g2_act;
d_act = D_FROM_IK(armId,d);
thp_act = THP_FROM_IK(armId,thp);
thy_act = THY_FROM_IK(armId,thy,grasp);
g1_act = FINGER1_FROM_IK(armId,thy,grasp);
g2_act = FINGER2_FROM_IK(armId,thy,grasp);
//check angles
int validity1[4];
bool valid1 = checkJointLimits1(d_act,thp_act,g1_act,g2_act,validity1);
if (!valid1) {
// if (_curr_rl == 3 && !(DISABLE_ALL_PRINTING)) {
// printf("ik %d invalid --1-- d [%d] % 2.4f \tp [%d] % 3.1f\ty [%d %d] % 3.1f\n",
// armId,
// validity1[0], d_act,
// validity1[1], thp_act RAD2DEG,
// validity1[2],validity1[3], thy_act RAD2DEG);
// }
return report;
}
//set joints
//setJointsWithLimits1(mech,d_act,thp_act,thy_act,grasp);
btVector3 z_roll_in_world = btTransform(Zi(d) * Xip * Zp(thp) * Xpy * Zy(thy) * Tg * Tgripper_to_world).invXform(btVector3(0,0,1));
btVector3 x_roll_in_world = btTransform(Zi(d) * Xip * Zp(thp) * Xpy * Zy(thy) * Tg * Tgripper_to_world).invXform(btVector3(1,0,0));
float zx = z_roll_in_world.x();
float zy = z_roll_in_world.y();
float zz = z_roll_in_world.z();
float xx = x_roll_in_world.x();
float xy = x_roll_in_world.y();
float xz = x_roll_in_world.z();
float cthe = (zy + kc12*kc23) / (ks12*ks23);
float the_1 = acos(cthe);
float the_2 = -acos(cthe);
float the_opt[2];
the_opt[0] = the_1;
the_opt[1] = the_2;
float ths_opt[2];
float thr_opt[2];
bool opts_valid[2];
float validity2[2][4];
float ths_act[2];
float the_act[2];
float thr_act[2];
for (int i=0;i<2;i++) {
float sthe_tmp = sin(the_opt[i]);
float C1 = ks12*kc23 + kc12*ks23*cthe;
float C2 = ks23 * sthe_tmp;
float C3 = C2 + C1*C1 / C2;
ths_opt[i] = atan2(
-sgn(C3)*(zx - C1 * zz / C2),
sgn(C3)*(zz + C1 * zx / C2));
float sths_tmp = sin(ths_opt[i]);
float cths_tmp = cos(ths_opt[i]);
float C4 = ks12 * sin(the_opt[i]);
float C5 = kc12 * ks23 + ks12 * kc23 * cos(the_opt[i]);
float C6 = kc23*(sthe_tmp * sths_tmp - kc12*cthe*cths_tmp) + cths_tmp*ks12*ks23;
float C7 = cthe*sths_tmp + kc12*cths_tmp*sthe_tmp;
thr_opt[i] = atan2(
(xx - C7 * xy / C4) / (C6 + C7*C5/C4),
(xx + C6 * xy / C5) / (-C6*C4/C5 - C7));
ths_act[i] = THS_FROM_IK(armId,ths_opt[i]);
the_act[i] = THE_FROM_IK(armId,the_opt[i]);
thr_act[i] = THR_FROM_IK(armId,thr_opt[i]);
// if (print) {
// log_msg("j s % 3.1f e % 3.1f r % 3.1f i % 1.3f p % 3.1f y % 3.1f g % 3.1f g1 % 3.1f g2 % 3.1f",
// arm->getJointById(Joint::Type::SHOULDER_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::ELBOW_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::TOOL_ROT_)->position() RAD2DEG,
// arm->getJointById(Joint::Type::INSERTION__)->position(),
// arm->getJointById(Joint::Type::WRIST_)->position() RAD2DEG,
// THY_MECH_FROM_FINGERS(armIdFromMechType(mech->type),arm->getJointById(Joint::Type::GRIPPER1_)->position(), arm->getJointById(Joint::Type::GRIPPER2_)->position()) RAD2DEG,//fix_angle(arm->getJointById(Joint::Type::GRIPPER2_)->position() - arm->getJointById(Joint::Type::GRIPPER1_)->position(),0) / 2 RAD2DEG,
// mech->ori.grasp * 1000. RAD2DEG,
// fix_angle(arm->getJointById(Joint::Type::GRIPPER1_)->position() + arm->getJointById(Joint::Type::GRIPPER2_)->position(),0) RAD2DEG,
// arm->getJointById(Joint::Type::GRIPPER1_)->position() RAD2DEG, arm->getJointById(Joint::Type::GRIPPER2_)->position() RAD2DEG);
// log_msg("%d s % 3.1f e % 3.1f r % 3.1f i % 1.3f p % 3.1f y % 3.1f g % 3.1f g1 % 3.1f g2 % 3.1f",i,
// ths_act[i] RAD2DEG,
// the_act[i] RAD2DEG,
// thr_act[i] RAD2DEG,
// d_act,
// thp_act RAD2DEG,
// thy_act RAD2DEG,
// grasp RAD2DEG,
// g1_act RAD2DEG,
// g2_act RAD2DEG);
//
// if (ths_act != ths_act) {
// log_msg("C1 %0.4f\tC2 %0.4f\tC3 %0.4f\tC4 %0.4f\tC5 %0.4f\tC6 %0.4f\tC7 %0.4f\t",C1,C2,C3,C4,C5,C6,C7);
// log_msg("ks23 %0.4f\tsthe_tmp %0.4f",ks23 , sthe_tmp);
// log_msg("cthe %0.4f\tzy %0.4f\tkc12 %0.4f\tkc23 %0.4f\tks12 %0.4f\tks23 %0.4f",cthe,zy,kc12,kc23,ks12,ks23);
// }
// }
bool valid2 = checkJointLimits2(ths_act[i],the_act[i],thr_act[i],validity2[i]);
opts_valid[i] = valid2;
if (valid2) {
float ths_diff, the_diff, d_diff, thr_diff, thp_diff, thg1_diff, thg2_diff;
//set joints
setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act);
setJointsWithLimits2(arm,ths_act[i],the_act[i],thr_act[i]);
ths_diff = arm->getJointById(Joint::IdType::SHOULDER_)->position() - ths_act[i];
the_diff = arm->getJointById(Joint::IdType::ELBOW_)->position() - the_act[i];
d_diff = arm->getJointById(Joint::IdType::INSERTION_)->position() - d_act;
thr_diff = arm->getJointById(Joint::IdType::ROTATION_)->position() - thr_act[i];
thp_diff = arm->getJointById(Joint::IdType::WRIST_)->position() - thp_act;
thg1_diff = arm->getJointById(Joint::IdType::FINGER1_)->position() - g1_act;
thg2_diff = arm->getJointById(Joint::IdType::FINGER2_)->position() - g2_act;
/*
ths_diff = arm->getJointById(Joint::Type::SHOULDER_)->position()_d - ths_act;
the_diff = arm->getJointById(Joint::Type::ELBOW_)->position()_d - the_act;
d_diff = arm->getJointById(Joint::Type::INSERTION__)->position()_d - d_act;
thr_diff = arm->getJointById(Joint::Type::TOOL_ROT_)->position()_d - thr_act;
thp_diff = arm->getJointById(Joint::Type::WRIST_)->position()_d - thp_act;
thg1_diff = arm->getJointById(Joint::Type::GRIPPER1_)->position()_d - g1_act;
thg2_diff = arm->getJointById(Joint::Type::GRIPPER2_)->position()_d - g2_act;
*/
// if (print) {
// log_msg("%d s % 2.1f e % 2.1f r % 2.1f i % 1.3f p % 2.1f g % 2.1f g1 % 2.1f g2 % 2.1f",i,
// arm->getJointById(Joint::Type::SHOULDER_)->position()_d RAD2DEG,
// arm->getJointById(Joint::Type::ELBOW_)->position()_d RAD2DEG,
// arm->getJointById(Joint::Type::TOOL_ROT_)->position()_d RAD2DEG,
// arm->getJointById(Joint::Type::INSERTION__)->position()_d,
// arm->getJointById(Joint::Type::WRIST_)->position()_d RAD2DEG,
// grasp RAD2DEG,
// arm->getJointById(Joint::Type::GRIPPER1_)->position()_d RAD2DEG,
// arm->getJointById(Joint::Type::GRIPPER2_)->position()_d RAD2DEG);
// log_msg("diff:");
// log_msg("R s %0.4f e %0.4f r %0.4f d %0.4f p %0.4f g1 %0.4f g2 %0.4f",
// ths_diff,the_diff,thr_diff,d_diff,thp_diff,thg1_diff,thg2_diff);
// log_msg("D s %0.4f e %0.4f r %0.4f d %0.4f p %0.4f g1 %0.4f g2 %0.4f",
// ths_diff*180/M_PI,the_diff*180/M_PI,thr_diff*180/M_PI,d_diff,
// thp_diff*180/M_PI,thg1_diff*180/M_PI,thg2_diff*180/M_PI);
//
// }
//
// if (i==1 && _curr_rl == 3) {
// //printf("ik ok! %d\n",_ik_counter);
// }
}
}
if (opts_valid[0]) {
report->success_ = true;
return report;
} else if (opts_valid[1]) {
// bool ENABLE_PARTIAL_INVALID_IK_PRINTING = false;
// if (ENABLE_PARTIAL_INVALID_IK_PRINTING && (_curr_rl == 3 || print) && !(DISABLE_ALL_PRINTING)) {
// printf("ik %d ok **2** s %1.4f %1.4f\te %1.4f %1.4f\tr %1.4f %1.4f\n",
// armId,
// ths_act[0] RAD2DEG,validity2[0][0],
// the_act[0] RAD2DEG,validity2[0][1],
// thr_act[0] RAD2DEG,validity2[0][2]);
// /*
// printf("%7d d %0.4f \tp %0.4f \ty %0.4f\n",_ik_counter,
// d_act,thp_act RAD2DEG,thy_act RAD2DEG);
// printf("x (%f, %f, %f) z (%f, %f, %f) %f\n",xx,xy,xz,zx,zy,zz,cthe);
// printf("norms %f %f\n",x_roll_in_world.length(),z_roll_in_world.length());
// printf("(zy + kc12*kc23): %f (%f, %f)\n",(zy + kc12*kc23),zy, kc12*kc23);
// printf("(ks12*ks23): %f\n",(ks12*ks23));
// */
// }
report->success_ = true;
return report;
} else {
const float maxValidDist = 3 DEG2RAD;
float valid_dist[2];
for (int i=0;i<2;i++) {
float sum = 0;
for (int j=0;j<3;j++) {
float v = fabs(validity2[i][j]);
sum += v*v;
}
valid_dist[i] = sqrt(sum);
}
bool use0 = valid_dist[0] < maxValidDist && valid_dist[0] < valid_dist[1];
bool use1 = valid_dist[1] < maxValidDist && valid_dist[0] > valid_dist[1];
printf("ik validity distances: (%s | %s) % 1.3f\t%f\n",use0?"Y":" ",use1?"Y":" ",valid_dist[0],valid_dist[1]);
if (valid_dist[0] < maxValidDist && valid_dist[0] < valid_dist[1]) {
printf("setting joints to ik soln 1\n");
setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act);
setJointsWithLimits2(arm,ths_act[0],the_act[0],thr_act[0]);
} else if (valid_dist[1] < maxValidDist) {
printf("setting joints to ik soln 2\n");
setJointsWithLimits1(arm,d_act,thp_act,g1_act,g2_act);
setJointsWithLimits2(arm,ths_act[1],the_act[1],thr_act[1]);
}
// if ((_curr_rl == 3 || print) && !(DISABLE_ALL_PRINTING)) {
// printf("ik %d invalid **2** s %1.4f % 2.1f\te %1.4f % 2.1f\tr %1.4f % 2.1f\n",
// armId,
// ths_act[0] RAD2DEG,validity2[0][0] RAD2DEG,
// the_act[0] RAD2DEG,validity2[0][1] RAD2DEG,
// thr_act[0] RAD2DEG,validity2[0][2] RAD2DEG);
// printf(" s %1.4f % 2.1f\te %1.4f % 2.1f\tr %1.4f % 2.1f\n",
// ths_act[1] RAD2DEG,validity2[1][0] RAD2DEG,
// the_act[1] RAD2DEG,validity2[1][1] RAD2DEG,
// thr_act[1] RAD2DEG,validity2[1][2] RAD2DEG);
// }
return report;
}
return report;
}
PhysicsObject* PhysicsMgr::createConvexHull(String meshname,Ogre::Vector3 pos,Ogre::Vector3 scale)
{
bool newshape = false;
size_t vertex_count;
float* vertices;
size_t index_count;
vertex_count = index_count = 0;
bool added_shared = false;
size_t current_offset = vertex_count;
size_t shared_offset = vertex_count;
size_t next_offset = vertex_count;
size_t index_offset = index_count;
size_t prev_vert = vertex_count;
size_t prev_ind = index_count;
std::vector<Ogre::Vector3> vertVect;
btConvexShape *convexShape;
//btShapeHull* hull;
if(meshname=="CUBE"||meshname=="BOX")
{
return createCube(scale,pos);
}
if(mShapes.find(meshname)==mShapes.end())
{
newshape = true;
Ogre::MeshPtr mesh = Ogre::MeshManager::getSingletonPtr()->load(meshname,Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
// Calculate how many vertices and indices we're going to need
for(int i = 0;i < mesh->getNumSubMeshes();i++)
{
Ogre::SubMesh* submesh = mesh->getSubMesh(i);
// We only need to add the shared vertices once
if(submesh->useSharedVertices)
{
if(!added_shared)
{
Ogre::VertexData* vertex_data = mesh->sharedVertexData;
vertex_count += vertex_data->vertexCount;
added_shared = true;
}
}
else
{
Ogre::VertexData* vertex_data = submesh->vertexData;
vertex_count += vertex_data->vertexCount;
}
// Add the indices
Ogre::IndexData* index_data = submesh->indexData;
index_count += index_data->indexCount;
}
// Allocate space for the vertices and indices
int a = vertex_count;
vertices = new float[vertex_count*3];
added_shared = false;
// Run through the submeshes again, adding the data into the arrays
for(int i = 0;i < mesh->getNumSubMeshes();i++)
{
Ogre::SubMesh* submesh = mesh->getSubMesh(i);
Ogre::VertexData* vertex_data = submesh->useSharedVertices ? mesh->sharedVertexData : submesh->vertexData;
if((!submesh->useSharedVertices)||(submesh->useSharedVertices && !added_shared))
{
if(submesh->useSharedVertices)
{
added_shared = true;
shared_offset = current_offset;
}
const Ogre::VertexElement* posElem = vertex_data->vertexDeclaration->findElementBySemantic(Ogre::VES_POSITION);
Ogre::HardwareVertexBufferSharedPtr vbuf = vertex_data->vertexBufferBinding->getBuffer(posElem->getSource());
unsigned char* vertex = static_cast<unsigned char*>(vbuf->lock(Ogre::HardwareBuffer::HBL_READ_ONLY));
Ogre::Real* pReal;
for(size_t j = 0; j < vertex_data->vertexCount; ++j, vertex += vbuf->getVertexSize())
{
posElem->baseVertexPointerToElement(vertex, &pReal);
Ogre::Vector3 pt;
pt.x = (*pReal++);
pt.y = (*pReal++);
pt.z = (*pReal++);
bool skip = false;
// ignore duped verts
for(unsigned int i=0;i<vertVect.size();++i)
{
if(vertVect[i]==pt)
{
skip = true;
//std::cout<<"IGNORED!\n";
}
}
// skipping duped verts caused some kind of f-up, TOBEFIXED eventually
if(!skip)
{
vertices[current_offset + (j*3)] = pt.x*0.92f;
vertices[current_offset + (j*3) + 1] = pt.y*0.92f;
vertices[current_offset + (j*3) + 2] = pt.z*0.92f;
vertVect.push_back(pt);
}
}
vbuf->unlock();
next_offset += vertex_data->vertexCount;
}
}
convexShape = new btConvexHullShape(static_cast<btScalar*>(vertices),vertVect.size(),3*sizeof(btScalar));
//create a hull approximation
btConvexShape *finalShape = 0;
//std::cout<<"size: "<<vertVect.size()<<"\n";
// seemed kinda iffy?
//if(vertVect.size()>75)
//{
// hull = new btShapeHull(convexShape);
// btScalar margin = convexShape->getMargin();
// hull->buildHull(margin);
// //btConvexHullShape* simplifiedConvexShape = new btConvexHullShape(hull->getVertexPointer(),hull->numVertices());
//
// btConvexHullShape* simplifiedConvexShape = new btConvexHullShape();
// for (int i=0;i<hull->numVertices();i++)
// {
// btVector3 vect = hull->getVertexPointer()[i]*0.9;
// std::cout<<"Vert: "<<vect.x()<<" "<<vect.y()<<" "<<vect.z()<<"\n";
// simplifiedConvexShape->addPoint(vect);
// }
// mShapes[meshname] = (simplifiedConvexShape);
// finalShape = simplifiedConvexShape;
//}
//else
//{
mShapes[meshname] = (convexShape);
finalShape = convexShape;
//}
}
//finalShape->setMargin(0.f);
btVector3 localInertia(0,0,0);
mShapes[meshname]->calculateLocalInertia(180.f,localInertia);
btRigidBody* actor = new btRigidBody(180.f,0,mShapes[meshname],localInertia);
actor->setWorldTransform(btTransform(btQuaternion::getIdentity(),btVector3(pos.x,pos.y,pos.z)));
actor->setRestitution(0.f);
actor->setFriction(0.8f);
//actor->setAnisotropicFriction(btVector3(0.3f,0.3f,0.3f));
actor->setDamping(0.2f,0.7f);
dynamic_cast<btDiscreteDynamicsWorld*>(mDynamicsWorld)->addRigidBody(actor,COLLISION_GROUP_1,COLLISION_GROUP_1);
mObjects.push_back(new PhysicsObject(actor,mDynamicsWorld));
if(newshape)
{
delete[] vertices;
//if(vertVect.size()>75)
//{
// delete hull;
// delete convexShape;
//}
}
return mObjects[mObjects.size()-1];
}
/* -----------------------------------------------------------------------
| build bullet height field shape and generate ogre mesh from grayscale image
|
| @param in :
| @param out: raw data of height field terrain
| ToDo: adjest grid scale, grid height, local scale, max/min height
----------------------------------------------------------------------- */
bool
buildHeightFieldTerrainFromImage(const Ogre::String& filename,
btDynamicsWorld* dynamicsWorld,
btAlignedObjectArray<btCollisionShape*>& collisionShapes,
void* &data)
{
Ogre::Image img;
try
{
img.load(filename, Ogre::ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME);
}
catch(Ogre::Exception err)
{
LOG(err.what());
return false;
}
size_t grid_w = 65, grid_h = 65; // must be (2^N) + 1
size_t grid_max_w = 129, grid_max_h = 129; // must be (2^N) + 1
size_t img_w = img.getWidth();
size_t img_h = img.getHeight();
// validate image size is (2^N) + 1
if ((img_w-1) & (img_w-2)) img_w = grid_w;
if ((img_h-1) & (img_h-2)) img_h = grid_h;
//if (img_w > grid_max_w) img_w = grid_max_w;
//if (img_h > grid_max_h) img_h = grid_max_h;
LOG("LoadImage name=%s, width=%d, height=%d, width^2+1=%d, height^2+1=%d",
filename.c_str(), img.getWidth(), img.getHeight(), img_w, img_h);
img.resize(img_w, img_h);
size_t pixelSize = Ogre::PixelUtil::getNumElemBytes(img.getFormat());
size_t bufSize = img.getSize() / pixelSize;
data = new Ogre::Real[ bufSize ];
Ogre::Real* dest = static_cast<Ogre::Real*>(data);
memset(dest, 0, bufSize);
/*
| @ Notice the alignment problem
| - uchar to float alignment
| - pixel format in bytes as rawdata type, also affects alignment
*/
Ogre::uchar* src = img.getData();
for (size_t i=0;i<bufSize;++i)
{
dest[i] = ((Ogre::Real)src[i * pixelSize] - 127.0f)/16.0f;
}
// parameter
int upAxis = 1;
btScalar gridSpacing = 5.0f;
btScalar gridHeightScale = 0.2f;
btScalar minHeight = -10.0f;
btScalar maxHeight = 10.0f;
btScalar defaultContactProcessingThreshold = BT_LARGE_FLOAT;
btHeightfieldTerrainShape *heightfieldShape =
new btHeightfieldTerrainShape(img_w, img_h,
dest,
gridHeightScale,
minHeight, maxHeight,
upAxis, PHY_FLOAT, false);
btAssert(heightfieldShape && "null heightfield");
// shape
btVector3 localScaling(1.0f, 1.0f, 1.0f);
heightfieldShape->setLocalScaling(localScaling);
collisionShapes.push_back(heightfieldShape);
// rigidBody
btDefaultMotionState* motionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,0,0)));
btRigidBody::btRigidBodyConstructionInfo cInfo(0, motionState, heightfieldShape, btVector3(0,0,0));
btRigidBody* rigidBody = new btRigidBody(cInfo);
rigidBody->setContactProcessingThreshold(defaultContactProcessingThreshold);
int flags = rigidBody->getCollisionFlags();
rigidBody->setCollisionFlags(flags | btCollisionObject::CF_DISABLE_VISUALIZE_OBJECT);
dynamicsWorld->addRigidBody(rigidBody);
// add ogre height field mesh
Ogre::SceneManager* sceneMgr = Ogre::Root::getSingletonPtr()->getSceneManager("DefaultSceneManager");
btAssert(sceneMgr);
Ogre::ManualObject* obj = sceneMgr->createManualObject("btHeightFieldEntity");
btVector3 aabbMin, aabbMax;
heightfieldShape->getAabb(btTransform(btQuaternion(0,0,0,1),btVector3(0,0,0)), aabbMin, aabbMax);
btHeightFieldProcessor callback(obj, "DefaultPlane");
heightfieldShape->processAllTriangles(&callback, aabbMin, aabbMax);
sceneMgr->getRootSceneNode()->attachObject(obj);
return true;
}
motion_state::motion_state()
{
node = NULL;
position_1 = btTransform();
}
int main( int argc, char* args[] )
{
//SDL
//Initialize all SDL subsystems
if( SDL_Init( SDL_INIT_EVERYTHING ) == -1 )
{
return 1;
}
//Set up the screen
screen = SDL_SetVideoMode( SCREEN_WIDTH, SCREEN_HEIGHT, SCREEN_BPP, SDL_SWSURFACE );
//If there was an error in setting up the screen
if( screen == NULL )
{
return 1;
}
//Set the window caption
SDL_WM_SetCaption( "Hello World", NULL );
//Load the images
message = load_image( "data/hello.bmp" );
background = load_image( "data/background.bmp" );
sphere = load_image("data/Circle.bmp");
//SDL
//Physics
//=> World Creation
// Specify the dynamic AABB tree broadphase algorithm to be used to work out what objects
// to test collision for.
btBroadphaseInterface* broadphase = new btDbvtBroadphase();
// The collision configuration allows you to fine tune the algorithms used
// for the full (not broadphase) collision detection. Here be dragons!
btDefaultCollisionConfiguration* collisionConfiguration =
new btDefaultCollisionConfiguration();
btCollisionDispatcher* dispatcher =
new btCollisionDispatcher(collisionConfiguration);
// We also need a "solver". This is what causes the objects to interact
// properly, taking into account gravity, game logic supplied forces,
// collisions, and hinge constraints. It does a good job as long as you
// don't push it to extremes, and is one of the bottlenecks in any high
// performance simulation. There are parallel versions available for some
// threading models.
btSequentialImpulseConstraintSolver* solver =
new btSequentialImpulseConstraintSolver;
// Now, we can finally instantiate the dynamics world.
btDiscreteDynamicsWorld* dynamicsWorld =
new btDiscreteDynamicsWorld(dispatcher, broadphase, solver,
collisionConfiguration);
// Set the gravity. We have chosen the Y axis to be "up".
dynamicsWorld->setGravity(btVector3(0, -10, 0));
//=> World Creation
//=> Ground Creation
// In this demonstration, we will place a ground plane running through
// the origin.
btCollisionShape* groundShape = new btStaticPlaneShape(
btVector3(0, 1, 0), 1);
// Instantiate the ground. Its orientation is the identity, Bullet quaternions
// are specified in x,y,z,w form. The position is 1 metre below the ground,
// which compensates the 1m offset we had to put into the shape itself.
btDefaultMotionState* groundMotionState = new btDefaultMotionState(
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, -1, 0)));
// The first and last parameters of the following constructor are the mass and
// inertia of the ground. Since the ground is static, we represent this by
// filling these values with zeros. Bullet considers passing a mass of zero
// equivalent to making a body with infinite mass - it is immovable.
btRigidBody::btRigidBodyConstructionInfo
groundRigidBodyCI(0, groundMotionState, groundShape, btVector3(0, 0, 0));
btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
//=> Ground Creation
//=> Sphere Creation
// The shape that we will let fall from the sky is a sphere with a radius
// of 1 metre.
btCollisionShape* fallShape = new btSphereShape(1);
// Adding the falling sphere is very similar. We will place it 50m above the
// ground.
btDefaultMotionState* fallMotionState =
new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1),
btVector3(0, 50, 0)));
// Since it's dynamic we will give it a mass of 1kg. I can't remember how to
// calculate the inertia of a sphere, but that doesn't matter because Bullet
// provides a utility function.
btScalar mass = 1;
btVector3 fallInertia(0, 0, 0);
fallShape->calculateLocalInertia(mass, fallInertia);
// Construct the rigid body just like before, and add it to the world.
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,
fallMotionState, fallShape, fallInertia);
btRigidBody* fallRigidBody = new btRigidBody(fallRigidBodyCI);
//=> Sphere Creation
// Add the ground and sphere to the world.
dynamicsWorld->addRigidBody(fallRigidBody);
dynamicsWorld->addRigidBody(groundRigidBody);
//Physics
bool go = true;
while (go){
SDL_Event incomingEvent;
while (SDL_PollEvent(&incomingEvent))
{
switch (incomingEvent.type)
{
case SDL_QUIT:
go = false;
break;
}
}
//Logic
//Physics
dynamicsWorld->stepSimulation(1 / 60.f, 10);
btTransform trans;
fallRigidBody->getMotionState()->getWorldTransform(trans);
//std::cout << "sphere height: " << trans.getOrigin().getY() << std::endl;
//Physics
//SDL
//Clear screen
SDL_FillRect(screen, NULL, 0x000000);
//Apply the background to the screen
apply_surface(0, 0, background, screen);
apply_surface(320, 0, background, screen);
apply_surface(0, 240, background, screen);
apply_surface(320, 240, background, screen);
//Apply the message to the screen
//apply_surface(180, 140, message, screen);
apply_surface(50, -(trans.getOrigin().getY()), sphere, screen);
//apply_surface(50, 50, sphere, screen);
std::cout << "X: " << trans.getOrigin().getX() << " Y: " << trans.getOrigin().getY() << std::endl;
//SDL
//Update
SDL_Flip(screen);
//Wait 2 seconds
SDL_Delay( 50 );
}
//Free the surfaces
SDL_FreeSurface( message );
SDL_FreeSurface( background );
//Quit SDL
SDL_Quit();
return 0;
}
Joint::Joint(){
trans = btTransform(btQuaternion::getIdentity(), btVector3(0.0,0.0,0.0));
points = NULL;
radius = 0.0;
numPoints = 0;
}
void processStateMenuEvents( int option ){
#if 0
vec3 motion;
switch( option ){
case 0:
options.view_mode = 4;
break;
case 1:
//reset score
options.p1_score = 0;
options.p2_score = 0;
options.hud.p1_score(options.p1_score);
options.hud.p2_score(options.p2_score);
//reset puck and paddle position
options.physics.puckRigidBody->setWorldTransform(btTransform(btQuaternion(1,0,0,0),btVector3(0, 0, 0)));
options.physics.paddle1RigidBody->setWorldTransform(btTransform(btQuaternion(1,0,0,0),btVector3(-3, 0, 0)));
options.physics.paddle2RigidBody->setWorldTransform(btTransform(btQuaternion(1,0,0,0),btVector3( 3, 0, 0)));
options.set_paddle1_dest(vec2(-3.0, 0.0));
options.set_paddle2_dest(vec2( 3.0, 0.0));
/* Start Puck */
//get an update of the motion state
options.physics.puckRigidBody->getMotionState()->getWorldTransform( options.physics.puck_trans );
//load new position into structure for paddle1 model
motion = vec3( options.physics.paddle1_trans.getOrigin().getX(), options.physics.paddle1_trans.getOrigin().getY(), options.physics.paddle1_trans.getOrigin().getZ());
//push new position into paddle1
options.puck->set_translation( motion );
/* End puck */
/* Start paddle 1 */
//get an update of the motion state
options.physics.paddle1RigidBody->getMotionState()->getWorldTransform( options.physics.paddle1_trans );
//load new position into structure for paddle1 model
motion = vec3( options.physics.paddle1_trans.getOrigin().getX(), options.physics.paddle1_trans.getOrigin().getY(), options.physics.paddle1_trans.getOrigin().getZ());
//push new position into paddle1
options.paddle1->set_translation( motion );
/* End paddle 1 */
/* Start paddle 2 */
//get an update of the motion state
options.physics.paddle2RigidBody->getMotionState()->getWorldTransform( options.physics.paddle2_trans );
//load new position into structure for paddle2 model
motion = vec3( options.physics.paddle2_trans.getOrigin().getX(), options.physics.paddle2_trans.getOrigin().getY(), options.physics.paddle2_trans.getOrigin().getZ());
//push new position into paddle2
options.paddle2->set_translation( motion );
/* End paddle 2 */
// reset velocities
options.physics.paddle1RigidBody->setLinearVelocity( btVector3(0.0,0.0,0.0));
options.physics.paddle1RigidBody->setAngularVelocity( btVector3(0.0,0.0,0.0));
options.physics.paddle2RigidBody->setLinearVelocity( btVector3(0.0,0.0,0.0));
options.physics.paddle2RigidBody->setAngularVelocity( btVector3(0.0,0.0,0.0));
options.physics.puckRigidBody->setLinearVelocity( btVector3(0.0,0.0,0.0));
options.physics.puckRigidBody->setAngularVelocity( btVector3(0.0,0.0,0.0));
break;
case 2:
options.view_mode = 0;
break;
}
#endif
}
//-------------------------------------------------------------------------------
void SlidingBrush::unpausedTick(const Ogre::FrameEvent &evt)
{
GraLL2GameObject::unpausedTick(evt);
if (mEnabled)
{
//Get old place (current place actually).
btTransform oldTrans;
mBody->getMotionState()->getWorldTransform(oldTrans);
btVector3 prevPos = oldTrans.getOrigin();
//Find next point position, and velocity.
int currPlace = mForward ? mCurrentPlace + 0.5 : mCurrentPlace -0.5;
Ogre::Vector3 currPoint = mPoints[currPlace];
Ogre::Vector3 velocity = (currPoint - BtOgre::Convert::toOgre(prevPos)).normalisedCopy() * mSpeed;
//Check if we're near next point.
Ogre::Real sqSpeed = mSpeed * mSpeed * evt.timeSinceLastFrame * evt.timeSinceLastFrame;
Ogre::Real sqDist = (currPoint - BtOgre::Convert::toOgre(prevPos)).squaredLength();
if (sqDist < sqSpeed)
{
//If near, jump to it.
mBody->getMotionState()->setWorldTransform(btTransform(oldTrans.getRotation(), BtOgre::Convert::toBullet(currPoint)));
mNode->setPosition(currPoint);
//If next 'place' exists, get there..
unsigned int nextPlace = currPlace + (mForward ? 1 : -1);
if (nextPlace >= 0 && nextPlace < mPoints.size())
mCurrentPlace += mForward ? 1 : -1;
//Call the Python 'point' event. Cut out repeated events using a 'last place' idea.
if (mLastPlace != currPlace)
NGF_PY_CALL_EVENT(point, currPlace);
mLastPlace = currPlace;
}
else
{
//Otherwise, usual velocity movement.
bool canMove = true;
btVector3 currVel = BtOgre::Convert::toBullet(velocity) * evt.timeSinceLastFrame;
btVector3 newPos = prevPos + currVel;
//If we're not ignoring collisions, check for 'em.
if (!mIgnoreCollisions)
{
//The cast result callback.
struct SlidingBrushCheckResult : public btDynamicsWorld::ConvexResultCallback
{
btCollisionObject *mIgnore;
int mDimension;
bool mHit;
Ogre::Real mHitFraction;
bool mYCast;
SlidingBrushCheckResult(btCollisionObject *ignore, int dimension, bool yCast)
: mIgnore(ignore),
mDimension(dimension),
mHit(false),
mHitFraction(1),
mYCast(yCast)
{
}
btScalar addSingleResult(btDynamicsWorld::LocalConvexResult &convexResult, bool)
{
mHit = true;
mHitFraction = convexResult.m_hitFraction < mHitFraction ? convexResult.m_hitFraction : mHitFraction;
return convexResult.m_hitFraction;
}
bool needsCollision(btBroadphaseProxy* proxy0) const
{
return ((btCollisionObject*) proxy0->m_clientObject != mIgnore) //Shouldn't be us.
&& (proxy0->m_collisionFilterGroup & mDimension) //Should be in our dimension.
&& (!(proxy0->m_collisionFilterGroup & DimensionManager::STATIC)) //No need to check with static.
&& !(mYCast && (proxy0->m_collisionFilterGroup & DimensionManager::LIFTABLE)) //If moving up, forget the liftables.
&& !(((btCollisionObject*) proxy0->m_clientObject)->getCollisionFlags() & btCollisionObject::CF_NO_CONTACT_RESPONSE); //If no contact response, ignore.
}
};
//Where to cast from, where to cast to, etc.
btVector3 normVel = currVel.normalized();
btVector3 pos1 = prevPos;
btVector3 pos2 = prevPos + currVel + (normVel * CAST_SHAPE_SHRINK);
btQuaternion rot = mBody->getWorldTransform().getRotation();
btTransform trans1(rot, pos1);
btTransform trans2(rot, pos2);
//Do the cast.
SlidingBrushCheckResult res(mBody, mDimensions, GlbVar.gravMgr->isUp() ? pos1.y() < pos2.y() : pos1.y() > pos2.y());
GlbVar.phyWorld->convexSweepTest(mCastShape, trans1, trans2, res);
//If hit, don't move.
if (res.mHit)
{
goto skip;
}
}
if (canMove)
{
oldTrans.setOrigin(newPos);
mBody->getMotionState()->setWorldTransform(oldTrans);
mNode->setPosition(BtOgre::Convert::toOgre(newPos));
}
}
}
skip:
//Python utick event.
NGF_PY_CALL_EVENT(utick, evt.timeSinceLastFrame);
}
/**
* Creates a randomly generated city.
*/
void City::Generate(void) {
for(int i = 0; i < CITY_W; i++) {
for(int j = 0; j < CITY_H; j++) {
hmap[i][j] = 1;
if(i == 0 || j == 0 || i == CITY_W-1 || j == CITY_H-1) {
hmap[i][j] = 0;
}
}
}
// Subdivide the place with streets (0 = a tile of street)
int max_divisions = CITY_W*CITY_H/10;
int divisions = 0;
int min_spread = 2;
int max_tries = CITY_W*CITY_H;
while(max_tries > 0 && divisions < max_divisions) {
int divx = rand()%CITY_W;
int divy = rand()%CITY_H;
// let's see if the nearest tiles are not street already
int street_check = 1;
for(int i = divx - min_spread; i < divx + min_spread; i++) {
for(int j = divy - min_spread; j < divy + min_spread; j++) {
if(i >= 0 && j >= 0 && i < CITY_W && j < CITY_H) {
street_check *= hmap[i][j];
}
}
}
if(street_check == 0) {
max_tries--;
continue;
}
for(int dir = 0; dir < 4; dir ++) {
int cx = divx;
int cy = divy;
int mxr = CITY_W > CITY_H ? CITY_W : CITY_H;
while(mxr > 0) {
// bumped into a street or border?
if(cx < 0 || cy < 0 || cx >= CITY_W || cy >= CITY_H
|| ( hmap[cx][cy] == 0 && (cx != divx || cy != divy) )) {
break;
}
// mark the tile as street
hmap[cx][cy] = 0;
// step accordingly (too haxy?)
cx += (dir+1)%2 * (1 - int(dir + .5)%3);
cy += (dir+2)%2 * (1 - int(dir + 2.5)%3);
mxr --;
}
}
divisions++;
max_tries--;
}
// Come up with some heights for the remaining buildings between streets
for(int i = 0; i < CITY_W; i++) {
for(int j = 0; j < CITY_H; j++) {
// Select a random height for the building, min 2
int height = 1+rand()%12;
int si, sj;
for(si = i; si < CITY_W; si++) {
for(sj = j; sj < CITY_H; sj++) {
if(hmap[si][sj] != 1) {
break;
}
hmap[si][sj] = height;
}
}
}
}
std::map<int, Block*> cache;
// Make bloxxx out of the height data!
for(int i = 0; i < CITY_W; i++) {
for(int j = 0; j < CITY_H; j++) {
for(int h = 0; h < getHeight(i,j); h++) {
Block * block = new Block(world, i, h, j, 1, 1, 1);
cache[ckey(i, j, h)] = block;
blocks.push_back(block);
if (h == 0) {
btPoint2PointConstraint * c = new btPoint2PointConstraint(
*block->body, btVector3(0, -0.5, 0));
block->body->addConstraintRef(c);
world.addConstraint(c, false);
} else if (h > 0) {
Block *bottom = *(blocks.end() - 2);
btQuaternion a;
a.setRotation(btVector3(0, 0, 1), 3.14159265358979/2);
btSliderConstraint * c = new btSliderConstraint(
*bottom->body, *block->body,
btTransform(a, btVector3(0, 0, 0)),
btTransform(a, btVector3(0, 0, 0)), true);
c->setLowerLinLimit(1);
c->setUpperLinLimit(1);
block->body->addConstraintRef(c);
world.addConstraint(c, true);
}
Block* left = cache[ckey(i-1, j, h)];
Block* back = cache[ckey(i, j-1, h)];
if(left) {
btQuaternion a(0, 0, 0, 1);
btSliderConstraint * c = new btSliderConstraint(
*left->body, *block->body,
btTransform::getIdentity(), btTransform::getIdentity(),
true);
c->setLowerLinLimit(1);
c->setUpperLinLimit(1);
block->body->addConstraintRef(c);
world.addConstraint(c, true);
}
if(back) {
btQuaternion a;
a.setRotation(btVector3(0, 1, 0), 3.14159265358979/2);
btSliderConstraint * c = new btSliderConstraint(
*block->body, *back->body,
btTransform(a, btVector3(0, 0, 0)),
btTransform(a, btVector3(0, 0, 0)), true);
c->setLowerLinLimit(1);
c->setUpperLinLimit(1);
block->body->addConstraintRef(c);
world.addConstraint(c, true);
}
}
}
}
}
virtual void getWorldTransform(btTransform& world_trans) const override
{
glm::vec3 pos = m_e.getComponent<spatial_component>().position;
glm::quat rot = m_e.getComponent<spatial_component>().rotation;
world_trans = btTransform(btQuaternion(rot.x, rot.y, rot.z, rot.w), btVector3(pos.x, pos.y, pos.z));
}
int main()
{
// Specify the dynamic AABB tree broadphase algorithm to be used to work out what objects
// to test collision for.
btBroadphaseInterface* broadphase = new btDbvtBroadphase();
// The collision configuration allows you to fine tune the algorithms used
// for the full (not broadphase) collision detection. Here be dragons!
btDefaultCollisionConfiguration* collisionConfiguration =
new btDefaultCollisionConfiguration();
btCollisionDispatcher* dispatcher =
new btCollisionDispatcher(collisionConfiguration);
// We also need a "solver". This is what causes the objects to interact
// properly, taking into account gravity, game logic supplied forces,
// collisions, and hinge constraints. It does a good job as long as you
// don't push it to extremes, and is one of the bottlenecks in any high
// performance simulation. There are parallel versions available for some
// threading models.
btSequentialImpulseConstraintSolver* solver =
new btSequentialImpulseConstraintSolver;
// Now, we can finally instantiate the dynamics world.
btDiscreteDynamicsWorld* dynamicsWorld =
new btDiscreteDynamicsWorld(dispatcher, broadphase, solver,
collisionConfiguration);
// Set the gravity. We have chosen the Y axis to be "up".
dynamicsWorld->setGravity(btVector3(0, -10, 0));
// In this demonstration, we will place a ground plane running through
// the origin.
btCollisionShape* groundShape = new btStaticPlaneShape(
btVector3(0, 1, 0), 1);
// The shape that we will let fall from the sky is a sphere with a radius
// of 1 metre.
btCollisionShape* fallShape = new btSphereShape(1);
// Instantiate the ground. Its orientation is the identity, Bullet quaternions
// are specified in x,y,z,w form. The position is 1 metre below the ground,
// which compensates the 1m offset we had to put into the shape itself.
btDefaultMotionState* groundMotionState = new btDefaultMotionState(
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, -1, 0)));
// The first and last parameters of the following constructor are the mass and
// inertia of the ground. Since the ground is static, we represent this by
// filling these values with zeros. Bullet considers passing a mass of zero
// equivalent to making a body with infinite mass - it is immovable.
btRigidBody::btRigidBodyConstructionInfo
groundRigidBodyCI(0, groundMotionState, groundShape, btVector3(0, 0, 0));
btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
// Add the ground to the world.
dynamicsWorld->addRigidBody(groundRigidBody);
// Adding the falling sphere is very similar. We will place it 50m above the
// ground.
btDefaultMotionState* fallMotionState =
new btDefaultMotionState(btTransform(btQuaternion(0, 0, 0, 1),
btVector3(0, 50, 0)));
// Since it's dynamic we will give it a mass of 1kg. I can't remember how to
// calculate the inertia of a sphere, but that doesn't matter because Bullet
// provides a utility function.
btScalar mass = 1;
btVector3 fallInertia(0, 0, 0);
fallShape->calculateLocalInertia(mass, fallInertia);
// Construct the rigid body just like before, and add it to the world.
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,
fallMotionState, fallShape, fallInertia);
btRigidBody* fallRigidBody = new btRigidBody(fallRigidBodyCI);
dynamicsWorld->addRigidBody(fallRigidBody);
// This is where the fun begins. Step the simulation 300 times, at an interval
// of 60hz. This will give the sphere enough time to hit the ground under the
// influence of gravity. Each step, we will print out its height above the
// ground.
for(int i = 0; i < 300; i++)
{
dynamicsWorld->stepSimulation(1 / 60.f, 10);
btTransform trans;
fallRigidBody->getMotionState()->getWorldTransform(trans);
std::cout << "sphere height: " << trans.getOrigin().getY() << std::endl;
}
// Clean up behind ourselves like good little programmers. Note that this is
// INCORRECT. We should be using unique_ptr<T> or shared_ptr<T> since we don't
// know if any of the above functions throw an exception.
dynamicsWorld->removeRigidBody(fallRigidBody);
delete fallRigidBody->getMotionState();
delete fallRigidBody;
dynamicsWorld->removeRigidBody(groundRigidBody);
delete groundRigidBody->getMotionState();
delete groundRigidBody;
delete fallShape;
delete groundShape;
delete dynamicsWorld;
delete solver;
delete collisionConfiguration;
delete dispatcher;
delete broadphase;
return 0;
}
GrabberKinematicObject::GrabberKinematicObject(float radius_, float height_) :
radius(radius_), height(height_),
constraintPivot(0, 0, height), // this is where objects will attach to
BulletObject(0, new btConeShapeZ(radius, height),
btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 0, 0)), true) {
}
osg::Group * createWorld() {
setupPerformanceStatistics();
//getPhysics()->setUseFixedTimeSteps(false);
//getPhysics()->setMaxSubSteps(5);
getPhysics()->addPerformanceStatistics(this);
cefix::log::setInfoLevel(osg::ALWAYS);
getMainWindow()->getCamera()->setClearColor(osg::Vec4(0.2, 0.2, 0.2, 1.0));
// statistics
if (0) {
enablePerformanceStatistics(true);
get2DLayer()->addChild(getPerformanceStatisticsGroup());
}
setUseOptimizerFlag(false);
osg::Group* g = new osg::Group();
{
//btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),1);
cefixbt::StaticPlaneShape* groundShape = new cefixbt::StaticPlaneShape(osg::Vec3(0,0,1),0);
cefixbt::MotionState* groundMotionState = new cefixbt::MotionState( btTransform(btQuaternion(0,0,0,1),btVector3(-250,-250,-1)) );
btRigidBody::btRigidBodyConstructionInfo groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
osg::ref_ptr<cefixbt::RigidBody> groundRigidBody = new cefixbt::RigidBody(groundRigidBodyCI);
groundRigidBody->setContactCallback(new GroundContactCallback());
addRigidBody(groundRigidBody);
osg::Geode* groundgeode = new osg::Geode();
groundgeode->addDrawable(new cefix::LineGridGeometry(osg::Vec3(500,500,0)));
groundMotionState->addChild(groundgeode);
g->addChild(groundMotionState);
}
for (unsigned int i = 0; i < 00; ++i) {
cefixbt::SphereShape* fallShape = new cefixbt::SphereShape(1);
cefixbt::MotionState* fallMotionState = new cefixbt::MotionState( btTransform(btQuaternion(0,0,0,1),btVector3(cefix::in_between(-5,5), cefix::in_between(-5,5), cefix::in_between(50,500))) );
btScalar mass = cefix::in_between(1,10);
btVector3 fallInertia(0,0,0);
fallShape->calculateLocalInertia(mass,fallInertia);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
osg::ref_ptr<cefixbt::RigidBody> fallRigidBody = new cefixbt::RigidBody(fallRigidBodyCI);
addRigidBody(fallRigidBody);
osg::Geode* fallgeode = new osg::Geode();
osg::ShapeDrawable* drawable = new osg::ShapeDrawable(fallShape);
drawable->setColor(osg::Vec4(cefix::randomf(1.0),cefix::randomf(1.0),cefix::randomf(1.0),1.0));
fallgeode->addDrawable(drawable);
fallMotionState->addChild(fallgeode);
g->addChild(fallMotionState);
}
std::vector<osg::ref_ptr<cefixbt::RigidBody> > boxes;
for (unsigned int i = 0; i < 00; ++i) {
cefixbt::BoxShape* fallShape = new cefixbt::BoxShape(osg::Vec3(1,1,1));
cefixbt::MotionState* fallMotionState = new cefixbt::MotionState( btTransform(btQuaternion(0,0,0,1),btVector3(cefix::in_between(-5,5), cefix::in_between(-5,5), cefix::in_between(50,500))) );
btScalar mass = cefix::in_between(1,10);
btVector3 fallInertia(0,0,0);
fallShape->calculateLocalInertia(mass,fallInertia);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
osg::ref_ptr<cefixbt::RigidBody> fallRigidBody = new cefixbt::RigidBody(fallRigidBodyCI);
addRigidBody(fallRigidBody);
osg::Geode* fallgeode = new osg::Geode();
osg::ShapeDrawable* drawable = new osg::ShapeDrawable(fallShape);
drawable->setColor(osg::Vec4(cefix::randomf(1.0),cefix::randomf(1.0),cefix::randomf(1.0),1.0));
fallgeode->addDrawable(drawable);
fallMotionState->addChild(fallgeode);
g->addChild(fallMotionState);
boxes.push_back(fallRigidBody);
}
if (1) {
osg::Node* node = osgDB::readNodeFile("w.obj");
osgUtil::Optimizer o;
o.optimize(node);
cefixbt::ConvexHullShape* fallShape = new cefixbt::ConvexHullShape(node, false);
//cefixbt::ConvexTriangleMeshShape* fallShape = new cefixbt::ConvexTriangleMeshShape(node, false);
osg::ref_ptr<cefixbt::RigidBody> last(NULL), current(NULL);
for (unsigned int i = 0; i < 10; ++i) {
cefixbt::MotionState* fallMotionState = new cefixbt::MotionState( btTransform(btQuaternion(0,0,0,1),btVector3(cefix::in_between(-50,50), cefix::in_between(-50,50), cefix::in_between(50,50))) );
btScalar mass = cefix::in_between(1,10);
btVector3 fallInertia(0,0,0);
fallShape->calculateLocalInertia(mass,fallInertia);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
osg::ref_ptr<cefixbt::RigidBody> fallRigidBody = new cefixbt::RigidBody(fallRigidBodyCI);
fallRigidBody->setDamping(0.4, 0.4);
addRigidBody(fallRigidBody);
fallMotionState->addChild(node);
fallMotionState->setUserData(new cefixbt::RigidBodyDraggable(fallRigidBody, this));
g->addChild(fallMotionState);
last = current;
current = fallRigidBody;
_rigidBodies.push_back(current);
if (last && current) {
const int mode(0);
switch (mode) {
case -1:
break;
case 0:
{
cefixbt::Point2PointConstraint* constraint = new cefixbt::Point2PointConstraint(last, current, osg::Vec3(7, 7, 0), osg::Vec3(-7, 0 , 0));
addConstraint(constraint);
}
break;
case 1:
{
cefixbt::Generic6DofConstraint* constraint = new cefixbt::Generic6DofConstraint(
last,
current,
osg::Matrix::translate(osg::Vec3(5, 0, 0)),
osg::Matrix::translate(osg::Vec3(-15, 0 , 0)),
true
);
constraint->setLinearLowerLimit(btVector3(0.0f, 0.0f, 0.0f));
constraint->setLinearUpperLimit(btVector3(3.0f, 0.0f, 0.0f));
constraint->setAngularLowerLimit(btVector3(0,0,0));
constraint->setAngularUpperLimit(btVector3(0.2,0,0));
addConstraint(constraint);
}
break;
case 2:
{
cefixbt::Generic6DofSpringConstraint* constraint = new cefixbt::Generic6DofSpringConstraint(
last,
current,
osg::Matrix::translate(osg::Vec3(5, 0, 0)),
osg::Matrix::translate(osg::Vec3(-15, 0 , 0)),
true
);
constraint->setLinearLowerLimit(btVector3(-1.0f, -1.0f, -1.0f));
constraint->setLinearUpperLimit(btVector3(1.0f, 1.0f, 1.0f));
constraint->setAngularLowerLimit(btVector3(-0.5,0,0));
constraint->setAngularUpperLimit(btVector3(0.5,0.0,0));
for(unsigned int i=0; i < 6; ++i) {
constraint->enableSpring(i, true);
constraint->setStiffness(i, 0.01);
constraint->setDamping (i, 0.01);
}
addConstraint(constraint);
}
}
}
}
}
osg::Group* world = new osg::Group();
world->addChild(g);
_scene = g;
// debug-world
if (1) {
_debugNode = getDebugDrawNode(
/*cefixbt::DebugPhysicsDrawer::DBG_DrawAabb | */
cefixbt::DebugPhysicsDrawer::DBG_DrawConstraints |
cefixbt::DebugPhysicsDrawer::DBG_DrawConstraintLimits |
cefixbt::DebugPhysicsDrawer::DBG_DrawFeaturesText |
cefixbt::DebugPhysicsDrawer::DBG_DrawContactPoints |
cefixbt::DebugPhysicsDrawer::DBG_NoDeactivation |
cefixbt::DebugPhysicsDrawer::DBG_NoHelpText |
cefixbt::DebugPhysicsDrawer::DBG_DrawText |
cefixbt::DebugPhysicsDrawer::DBG_ProfileTimings |
cefixbt::DebugPhysicsDrawer::DBG_DrawWireframe
);
world->addChild(_debugNode);
_debugNode->setNodeMask(0x0);
}
return world;
}
void btCollisionWorld::objectQuerySingle(const btConvexShape* castShape,const btTransform& convexFromTrans,const btTransform& convexToTrans,
btCollisionObject* collisionObject,
const btCollisionShape* collisionShape,
const btTransform& colObjWorldTransform,
ConvexResultCallback& resultCallback, btScalar allowedPenetration)
{
if (collisionShape->isConvex())
{
//BT_PROFILE("convexSweepConvex");
btConvexCast::CastResult castResult;
castResult.m_allowedPenetration = allowedPenetration;
castResult.m_fraction = resultCallback.m_closestHitFraction;//btScalar(1.);//??
btConvexShape* convexShape = (btConvexShape*) collisionShape;
btVoronoiSimplexSolver simplexSolver;
btGjkEpaPenetrationDepthSolver gjkEpaPenetrationSolver;
btContinuousConvexCollision convexCaster1(castShape,convexShape,&simplexSolver,&gjkEpaPenetrationSolver);
//btGjkConvexCast convexCaster2(castShape,convexShape,&simplexSolver);
//btSubsimplexConvexCast convexCaster3(castShape,convexShape,&simplexSolver);
btConvexCast* castPtr = &convexCaster1;
if (castPtr->calcTimeOfImpact(convexFromTrans,convexToTrans,colObjWorldTransform,colObjWorldTransform,castResult))
{
//add hit
if (castResult.m_normal.length2() > btScalar(0.0001))
{
if (castResult.m_fraction < resultCallback.m_closestHitFraction)
{
castResult.m_normal.normalize();
btCollisionWorld::LocalConvexResult localConvexResult
(
collisionObject,
0,
castResult.m_normal,
castResult.m_hitPoint,
castResult.m_fraction
);
bool normalInWorldSpace = true;
resultCallback.addSingleResult(localConvexResult, normalInWorldSpace);
}
}
}
} else {
if (collisionShape->isConcave())
{
if (collisionShape->getShapeType()==TRIANGLE_MESH_SHAPE_PROXYTYPE)
{
//BT_PROFILE("convexSweepbtBvhTriangleMesh");
btBvhTriangleMeshShape* triangleMesh = (btBvhTriangleMeshShape*)collisionShape;
btTransform worldTocollisionObject = colObjWorldTransform.inverse();
btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
//ConvexCast::CastResult
struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
{
btCollisionWorld::ConvexResultCallback* m_resultCallback;
btCollisionObject* m_collisionObject;
btTriangleMeshShape* m_triangleMesh;
BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btTriangleMeshShape* triangleMesh, const btTransform& triangleToWorld):
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
m_resultCallback(resultCallback),
m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh)
{
}
virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
{
btCollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex;
if (hitFraction <= m_resultCallback->m_closestHitFraction)
{
btCollisionWorld::LocalConvexResult convexResult
(m_collisionObject,
&shapeInfo,
hitNormalLocal,
hitPointLocal,
hitFraction);
bool normalInWorldSpace = true;
return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
}
return hitFraction;
}
};
BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,triangleMesh, colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
btVector3 boxMinLocal, boxMaxLocal;
castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
triangleMesh->performConvexcast(&tccb,convexFromLocal,convexToLocal,boxMinLocal, boxMaxLocal);
} else
{
//BT_PROFILE("convexSweepConcave");
btConcaveShape* concaveShape = (btConcaveShape*)collisionShape;
btTransform worldTocollisionObject = colObjWorldTransform.inverse();
btVector3 convexFromLocal = worldTocollisionObject * convexFromTrans.getOrigin();
btVector3 convexToLocal = worldTocollisionObject * convexToTrans.getOrigin();
// rotation of box in local mesh space = MeshRotation^-1 * ConvexToRotation
btTransform rotationXform = btTransform(worldTocollisionObject.getBasis() * convexToTrans.getBasis());
//ConvexCast::CastResult
struct BridgeTriangleConvexcastCallback : public btTriangleConvexcastCallback
{
btCollisionWorld::ConvexResultCallback* m_resultCallback;
btCollisionObject* m_collisionObject;
btConcaveShape* m_triangleMesh;
BridgeTriangleConvexcastCallback(const btConvexShape* castShape, const btTransform& from,const btTransform& to,
btCollisionWorld::ConvexResultCallback* resultCallback, btCollisionObject* collisionObject,btConcaveShape* triangleMesh, const btTransform& triangleToWorld):
btTriangleConvexcastCallback(castShape, from,to, triangleToWorld, triangleMesh->getMargin()),
m_resultCallback(resultCallback),
m_collisionObject(collisionObject),
m_triangleMesh(triangleMesh)
{
}
virtual btScalar reportHit(const btVector3& hitNormalLocal, const btVector3& hitPointLocal, btScalar hitFraction, int partId, int triangleIndex )
{
btCollisionWorld::LocalShapeInfo shapeInfo;
shapeInfo.m_shapePart = partId;
shapeInfo.m_triangleIndex = triangleIndex;
if (hitFraction <= m_resultCallback->m_closestHitFraction)
{
btCollisionWorld::LocalConvexResult convexResult
(m_collisionObject,
&shapeInfo,
hitNormalLocal,
hitPointLocal,
hitFraction);
bool normalInWorldSpace = false;
return m_resultCallback->addSingleResult(convexResult,normalInWorldSpace);
}
return hitFraction;
}
};
BridgeTriangleConvexcastCallback tccb(castShape, convexFromTrans,convexToTrans,&resultCallback,collisionObject,concaveShape, colObjWorldTransform);
tccb.m_hitFraction = resultCallback.m_closestHitFraction;
btVector3 boxMinLocal, boxMaxLocal;
castShape->getAabb(rotationXform, boxMinLocal, boxMaxLocal);
btVector3 rayAabbMinLocal = convexFromLocal;
rayAabbMinLocal.setMin(convexToLocal);
btVector3 rayAabbMaxLocal = convexFromLocal;
rayAabbMaxLocal.setMax(convexToLocal);
rayAabbMinLocal += boxMinLocal;
rayAabbMaxLocal += boxMaxLocal;
concaveShape->processAllTriangles(&tccb,rayAabbMinLocal,rayAabbMaxLocal);
}
} else {
///@todo : use AABB tree or other BVH acceleration structure!
if (collisionShape->isCompound())
{
BT_PROFILE("convexSweepCompound");
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(collisionShape);
int i=0;
for (i=0;i<compoundShape->getNumChildShapes();i++)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* childCollisionShape = compoundShape->getChildShape(i);
btTransform childWorldTrans = colObjWorldTransform * childTrans;
// replace collision shape so that callback can determine the triangle
btCollisionShape* saveCollisionShape = collisionObject->getCollisionShape();
collisionObject->internalSetTemporaryCollisionShape((btCollisionShape*)childCollisionShape);
objectQuerySingle(castShape, convexFromTrans,convexToTrans,
collisionObject,
childCollisionShape,
childWorldTrans,
resultCallback, allowedPenetration);
// restore
collisionObject->internalSetTemporaryCollisionShape(saveCollisionShape);
}
}
}
}
}
int main (void)
{
// Setup
btBroadphaseInterface* broadphase = new btDbvtBroadphase();
btDefaultCollisionConfiguration* collisionConfiguration = new btDefaultCollisionConfiguration();
btCollisionDispatcher* dispatcher = new btCollisionDispatcher(collisionConfiguration);
btSequentialImpulseConstraintSolver* solver = new btSequentialImpulseConstraintSolver;
btDiscreteDynamicsWorld* dynamicsWorld = new btDiscreteDynamicsWorld(dispatcher,broadphase,solver,collisionConfiguration);
dynamicsWorld->setGravity(btVector3(0,-10,0));
btCollisionShape* groundShape = new btStaticPlaneShape(btVector3(0,1,0),1);
btCollisionShape* fallShape = new btSphereShape(1);
btDefaultMotionState* groundMotionState = new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,-1,0)));
btRigidBody::btRigidBodyConstructionInfo
groundRigidBodyCI(0,groundMotionState,groundShape,btVector3(0,0,0));
btRigidBody* groundRigidBody = new btRigidBody(groundRigidBodyCI);
dynamicsWorld->addRigidBody(groundRigidBody);
btDefaultMotionState* fallMotionState =
new btDefaultMotionState(btTransform(btQuaternion(0,0,0,1),btVector3(0,50,0)));
btScalar mass = 1;
btVector3 fallInertia(0,0,0);
fallShape->calculateLocalInertia(mass,fallInertia);
btRigidBody::btRigidBodyConstructionInfo fallRigidBodyCI(mass,fallMotionState,fallShape,fallInertia);
btRigidBody* fallRigidBody = new btRigidBody(fallRigidBodyCI);
dynamicsWorld->addRigidBody(fallRigidBody);
for (int i=0 ; i<300 ; i++) {
dynamicsWorld->stepSimulation(1/60.f,10);
btTransform trans;
fallRigidBody->getMotionState()->getWorldTransform(trans);
std::cout << "sphere height: " << trans.getOrigin().getY() << std::endl;
}
// Clean up
dynamicsWorld->removeRigidBody(fallRigidBody);
delete fallRigidBody->getMotionState();
delete fallRigidBody;
dynamicsWorld->removeRigidBody(groundRigidBody);
delete groundRigidBody->getMotionState();
delete groundRigidBody;
delete fallShape;
delete groundShape;
delete dynamicsWorld;
delete solver;
delete collisionConfiguration;
delete dispatcher;
delete broadphase;
return 0;
}
Ragdoll::Ragdoll(btDiscreteDynamicsWorld * world, btScalar heightOffset)
{
this->world = world;
btScalar bodyMass = (btScalar)70.0;
// feet definition
btScalar footLength = (btScalar)0.24, footHeight = (btScalar)0.08, footWidth = (btScalar)0.15;
btScalar footTop = footHeight + heightOffset;
btScalar footXOffset = (btScalar)0.04, footZOffset = (btScalar)0.167;
btScalar footMass = bodyMass * (btScalar)1.38/100;
// left foot
btCollisionShape *bpShape = new btBoxShape(btVector3(footLength/2, footHeight/2, footWidth/2));
btQuaternion bpRotation(0, 0, 0, 1);
btVector3 bpTranslation(footXOffset, footTop-footHeight/2, -footZOffset);
btDefaultMotionState *bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
btVector3 bpInertia(0, 0, 0);
btScalar bpMass = footMass;
bpShape->calculateLocalInertia(bpMass, bpInertia);
btRigidBody::btRigidBodyConstructionInfo rbInfo(bpMass, bpMotionState, bpShape, bpInertia);
btRigidBody *body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_LEFT_FOOT] = new GrBulletObject(body);
addBoxLinker(footLength / 2, footHeight / 2, footWidth / 2, bodyParts[BODYPART_LEFT_FOOT]);
// right foot
bpShape = new btBoxShape(btVector3(footLength / 2, footHeight / 2, footWidth / 2));
bpRotation = btQuaternion(0, 0, 0, 1);
bpTranslation = btVector3(footXOffset, footTop - footHeight / 2, footZOffset);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = footMass;
bpShape->calculateLocalInertia(bpMass, bpInertia);
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_RIGHT_FOOT] = new GrBulletObject(body);
addBoxLinker(footLength / 2, footHeight / 2, footWidth / 2, bodyParts[BODYPART_RIGHT_FOOT]);
// legs definition
btScalar legRadius = (btScalar)0.055, legHeight = (btScalar) 0.34;
btScalar legTop = footTop + legHeight;
btScalar legMass = bodyMass * (btScalar)5.05/100;
// left leg
bpShape = new btCapsuleShape(legRadius, legHeight - 2 * legRadius);
bpRotation = btQuaternion(0, 0, 0, 1);
bpTranslation = btVector3(0, legTop - legHeight / 2, -footZOffset);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = legMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_LEFT_LEG] = new GrBulletObject(body);
addCylinderLinker(legRadius, legHeight, bodyParts[BODYPART_LEFT_LEG]);
// right leg
bpShape = new btCapsuleShape(legRadius, legHeight - 2 * legRadius);
bpRotation = btQuaternion(0, 0, 0, 1);
bpTranslation = btVector3(0, legTop - legHeight / 2, footZOffset);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = legMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_RIGHT_LEG] = new GrBulletObject(body);
addCylinderLinker(legRadius, legHeight, bodyParts[BODYPART_RIGHT_LEG]);
// thighs definition
btScalar thighRadius = (btScalar)0.07, thighHeight = (btScalar)0.32;
btScalar thighAngle = (btScalar) (12 * M_PI / 180);
btScalar thighAngledHeight = thighHeight * btCos(thighAngle);
btScalar thighTop = legTop + thighAngledHeight;
btScalar thighZOffset = (btScalar)0.1136;
btScalar thighMass = bodyMass * (btScalar)11.125/100;
// left thigh h=0.316
bpShape = new btCapsuleShape(thighRadius, thighHeight - 2 * thighRadius);
bpRotation = btQuaternion(1 * btSin(thighAngle / 2), 0, 0, btCos(thighAngle / 2));
bpTranslation = btVector3(0, thighTop - thighAngledHeight/2, -thighZOffset);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = thighMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_LEFT_THIGH] = new GrBulletObject(body);
addCylinderLinker(thighRadius, thighHeight, bodyParts[BODYPART_LEFT_THIGH]);
// right thigh h=0.316 , ends at 0.706
bpShape = new btCapsuleShape(thighRadius, thighHeight - 2 * thighRadius);
bpRotation = btQuaternion(-1 * btSin(thighAngle / 2), 0, 0, btCos(thighAngle / 2));
bpTranslation = btVector3(0, thighTop - thighAngledHeight / 2, thighZOffset);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = thighMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_RIGHT_THIGH] = new GrBulletObject(body);
addCylinderLinker(thighRadius, thighHeight, bodyParts[BODYPART_RIGHT_THIGH]);
// pelvis definition
btScalar pelvisLength = (btScalar)0.19, pelvisHeight = (btScalar)0.15, pelvisWidth = (btScalar)0.35;
btScalar pelvisTop = thighTop + pelvisHeight;
btScalar pelvisMass = bodyMass * (btScalar)14.81/100;
// pelvis
bpShape = new btBoxShape(btVector3(pelvisLength/2, pelvisHeight/2, pelvisWidth/2));
bpRotation = btQuaternion(0, 0, 0, 1);
bpTranslation = btVector3(0, pelvisTop - pelvisHeight/2, 0);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = pelvisMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_PELVIS] = new GrBulletObject(body);
addBoxLinker(pelvisLength / 2, pelvisHeight / 2, pelvisWidth / 2, bodyParts[BODYPART_PELVIS]);
// abdomen definition
btScalar abdomenLength = (btScalar)0.13, abdomenHeight = (btScalar)0.113, abdomenWidth = (btScalar)0.268;
btScalar abdomenTop = pelvisTop + abdomenHeight;
btScalar abdomenMass = bodyMass * (btScalar)12.65 / 100;
// abdomen
bpShape = new btBoxShape(btVector3(abdomenLength/2, abdomenHeight/2, abdomenWidth/2));
bpRotation = btQuaternion(0, 0, 0, 1);
bpTranslation = btVector3(0, abdomenTop - abdomenHeight/2, 0);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = abdomenMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_ABDOMEN] = new GrBulletObject(body);
addBoxLinker(abdomenLength / 2, abdomenHeight / 2, abdomenWidth / 2, bodyParts[BODYPART_ABDOMEN]);
// thorax definition
btScalar thoraxLength = (btScalar)0.2, thoraxHeight = (btScalar)0.338, thoraxWidth = (btScalar)0.34;
btScalar thoraxTop = abdomenTop + thoraxHeight;
btScalar thoraxMass = bodyMass * (btScalar)18.56/100;
// thorax
bpShape = new btBoxShape(btVector3(thoraxLength/2, thoraxHeight/2, thoraxWidth/2));
bpRotation = btQuaternion(0, 0, 0, 1);
bpTranslation = btVector3(0, thoraxTop-thoraxHeight/2, 0);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = thoraxMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_THORAX] = new GrBulletObject(body);
addBoxLinker(thoraxLength / 2, thoraxHeight / 2, thoraxWidth / 2, bodyParts[BODYPART_THORAX]);
// upper arms definition
btScalar upperArmRadius = (btScalar)0.04, upperArmHeight = (btScalar)0.25;
btScalar upperArmAngle = (btScalar)(25 * M_PI / 180);
btScalar upperArmAngledHeight = upperArmHeight * btCos(upperArmAngle);
btScalar upperArmBottom = thoraxTop - upperArmAngledHeight;
btScalar upperArmZOffset = (btScalar)0.223;
btScalar upperArmMass = bodyMass * (btScalar)3.075 / 100;
// left upper arm
bpShape = new btCapsuleShape(upperArmRadius, upperArmHeight - 2 * upperArmRadius);
bpShape = new btBoxShape(btVector3(upperArmRadius, upperArmHeight/2, upperArmRadius));
bpRotation = btQuaternion(1 * btSin(upperArmAngle/2), 0, 0, btCos(upperArmAngle/2));
bpTranslation = btVector3(0, upperArmBottom + upperArmAngledHeight/2, -upperArmZOffset);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = upperArmMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_LEFT_UPPER_ARM] = new GrBulletObject(body);
addCylinderLinker(upperArmRadius, upperArmHeight, bodyParts[BODYPART_LEFT_UPPER_ARM]);
// right upper arm
bpShape = new btCapsuleShape(upperArmRadius, upperArmHeight - 2 * upperArmRadius);
bpRotation = btQuaternion(-1 * btSin(upperArmAngle / 2), 0, 0, btCos(upperArmAngle / 2));
bpTranslation = btVector3(0, upperArmBottom + upperArmAngledHeight / 2, upperArmZOffset);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = upperArmMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_RIGHT_UPPER_ARM] = new GrBulletObject(body);
addCylinderLinker(upperArmRadius, upperArmHeight, bodyParts[BODYPART_RIGHT_UPPER_ARM]);
// lower arms definition
btScalar lowerArmRadius = (btScalar)0.035, lowerArmHeight = (btScalar)0.28;
btScalar lowerArmTop = upperArmBottom;
btScalar lowerArmZOffset = (btScalar)0.276;
btScalar lowerArmMass = bodyMass * (btScalar)1.72 / 100;
// left lower arm
bpShape = new btCapsuleShape(lowerArmRadius, lowerArmHeight - 2 * lowerArmRadius);
bpRotation = btQuaternion(0, 0, 0, 1);
bpTranslation = btVector3(0, lowerArmTop - lowerArmHeight/2, -lowerArmZOffset);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = lowerArmMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_LEFT_LOWER_ARM] = new GrBulletObject(body);
addCylinderLinker(lowerArmRadius, lowerArmHeight, bodyParts[BODYPART_LEFT_LOWER_ARM]);
// right lower arm
bpShape = new btCapsuleShape(lowerArmRadius, lowerArmHeight - 2 * lowerArmRadius);
bpRotation = btQuaternion(0, 0, 0, 1);
bpTranslation = btVector3(0, lowerArmTop - lowerArmHeight / 2, lowerArmZOffset);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = lowerArmMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_RIGHT_LOWER_ARM] = new GrBulletObject(body);
addCylinderLinker(lowerArmRadius, lowerArmHeight, bodyParts[BODYPART_RIGHT_LOWER_ARM]);
// neck definition
btScalar neckRadius = (btScalar)0.05, neckHeight = (btScalar)0.04;
btScalar neckTop = thoraxTop + neckHeight;
btScalar neckMass = (btScalar)0.5;
// neck
bpShape = new btBoxShape(btVector3(neckRadius, neckHeight/2, neckRadius));
bpRotation = btQuaternion(0, 0, 0, 1);
bpTranslation = btVector3(0, neckTop - neckHeight/2, 0);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = neckMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_NECK] = new GrBulletObject(body);
addCylinderLinker(neckRadius, neckHeight, bodyParts[BODYPART_NECK]);
// head definition
btScalar headRadius = (btScalar)0.1, headHeight = (btScalar)0.283;
btScalar headTop = neckTop + headHeight;
btScalar headMass = (btScalar)5.0;
// head
bpShape = new btCapsuleShape(headRadius, headHeight-2*headRadius);
bpRotation = btQuaternion(0, 0, 0, 1);
bpTranslation = btVector3(0, headTop - headHeight/2, 0);
bpMotionState = new btDefaultMotionState(btTransform(bpRotation, bpTranslation));
bpInertia = btVector3(0, 0, 0);
bpMass = headMass;
rbInfo = btRigidBody::btRigidBodyConstructionInfo(bpMass, bpMotionState, bpShape, bpInertia);
body = new btRigidBody(rbInfo);
world->addRigidBody(body);
bodyParts[BODYPART_HEAD] = new GrBulletObject(body);
addSphereLinker(headHeight/2, bodyParts[BODYPART_HEAD]);
// create joints
btConeTwistConstraint *coneConstraint;
btHingeConstraint * hingeConstraint;
// head-neck
btTransform bodyA, bodyB;
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, 0, M_PI_2);
bodyA.setOrigin(btVector3(0, -(headHeight/2 + 0.02), 0));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, 0, M_PI_2);
bodyB.setOrigin(btVector3(0, neckHeight/2 + 0.01, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_HEAD]->getRigidBody(), *bodyParts[BODYPART_NECK]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_4, M_PI_4, M_PI_2);
joints[JOINT_HEAD_NECK] = coneConstraint;
world->addConstraint(joints[JOINT_HEAD_NECK], false);
// neck-thorax
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, 0, M_PI_2);
bodyA.setOrigin(btVector3(0, -neckHeight/2 - 0.02, 0));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, 0, M_PI_2);
bodyB.setOrigin(btVector3(0, thoraxHeight / 2 + 0.02, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_NECK]->getRigidBody(), *bodyParts[BODYPART_THORAX]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
joints[JOINT_NECK_THORAX] = coneConstraint;
world->addConstraint(joints[JOINT_NECK_THORAX], true);
// thorax-leftupperarm
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, thoraxHeight / 2 - upperArmRadius, -(thoraxWidth / 2 + upperArmRadius)));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(0, upperArmHeight / 2 + upperArmRadius, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_THORAX]->getRigidBody(), *bodyParts[BODYPART_LEFT_UPPER_ARM]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_2, M_PI_2, 0);
joints[JOINT_THORAX_LEFT_UPPER_ARM] = coneConstraint;
world->addConstraint(joints[JOINT_THORAX_LEFT_UPPER_ARM], true);
// left upper-lower arm
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, -(upperArmHeight / 2 + upperArmRadius / 2), 0));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(0, lowerArmHeight / 2 + lowerArmRadius / 2, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_LEFT_UPPER_ARM]->getRigidBody(), *bodyParts[BODYPART_LEFT_LOWER_ARM]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_2, M_PI_2, M_PI_4);
joints[JOINT_LEFT_ARM_UPPER_LOWER] = coneConstraint;
world->addConstraint(joints[JOINT_LEFT_ARM_UPPER_LOWER], true);
// thorax-rightupperarm
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, -M_PI_2, 0);
bodyA.setOrigin(btVector3(0, thoraxHeight / 2 - upperArmRadius, (thoraxWidth / 2 + upperArmRadius)));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, -M_PI_2, 0);
bodyB.setOrigin(btVector3(0, upperArmHeight / 2 + upperArmRadius, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_THORAX]->getRigidBody(), *bodyParts[BODYPART_RIGHT_UPPER_ARM]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_2, M_PI_2, 0);
joints[JOINT_THORAX_RIGHT_UPPER_ARM] = coneConstraint;
world->addConstraint(joints[JOINT_THORAX_RIGHT_UPPER_ARM], true);
// right upper-lower arm
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, -(upperArmHeight / 2 + upperArmRadius / 2), 0));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(0, lowerArmHeight / 2 + lowerArmRadius / 2, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_RIGHT_UPPER_ARM]->getRigidBody(), *bodyParts[BODYPART_RIGHT_LOWER_ARM]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_2, M_PI_2, M_PI_4);
joints[JOINT_RIGHT_ARM_UPPER_LOWER] = coneConstraint;
world->addConstraint(joints[JOINT_RIGHT_ARM_UPPER_LOWER], true);
// thorax-abdomen
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, -(thoraxHeight/2 - 0.05), 0));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(0, abdomenHeight/2 + 0.05, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_THORAX]->getRigidBody(), *bodyParts[BODYPART_ABDOMEN]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
joints[JOINT_THORAX_ADBOMEN] = coneConstraint;
world->addConstraint(joints[JOINT_THORAX_ADBOMEN], true);
// abdomen-pelvis
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, -(abdomenHeight/2 - 0.05), 0));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(0, pelvisHeight/2 + 0.05, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_ABDOMEN]->getRigidBody(), *bodyParts[BODYPART_PELVIS]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
joints[JOINT_ABDOMEN_PELVIS] = coneConstraint;
world->addConstraint(joints[JOINT_ABDOMEN_PELVIS], true);
// pelvis-leftthigh
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, -(pelvisHeight / 2), -(pelvisWidth/2 - thighRadius)));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(0, thighHeight/2 + thighRadius, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_PELVIS]->getRigidBody(), *bodyParts[BODYPART_LEFT_THIGH]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
joints[JOINT_PELVIS_LEFT_THIGH] = coneConstraint;
world->addConstraint(joints[JOINT_PELVIS_LEFT_THIGH], true);
// left thigh-leg
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, -(thighHeight / 2 + thighRadius), 0));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(0, legHeight / 2 + legRadius, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_LEFT_THIGH]->getRigidBody(), *bodyParts[BODYPART_LEFT_LEG]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
joints[JOINT_LEFT_THIGH_LEG] = coneConstraint;
world->addConstraint(joints[JOINT_LEFT_THIGH_LEG], true);
// left leg-foot
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, -(legHeight / 2 + legRadius), 0));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(-footXOffset, footHeight + 0.03, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_LEFT_LEG]->getRigidBody(), *bodyParts[BODYPART_LEFT_FOOT]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
joints[JOINT_LEFT_LEG_FOOT] = coneConstraint;
world->addConstraint(joints[JOINT_LEFT_LEG_FOOT], true);
// pelvis-rightthigh
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, -(pelvisHeight / 2), (pelvisWidth / 2 - thighRadius)));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(0, thighHeight / 2 + thighRadius, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_PELVIS]->getRigidBody(), *bodyParts[BODYPART_RIGHT_THIGH]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
joints[JOINT_PELVIS_RIGHT_THIGH] = coneConstraint;
world->addConstraint(joints[JOINT_PELVIS_RIGHT_THIGH], true);
// RIGHT thigh-leg
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, -(thighHeight / 2 + thighRadius), 0));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(0, legHeight / 2 + legRadius, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_RIGHT_THIGH]->getRigidBody(), *bodyParts[BODYPART_RIGHT_LEG]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
joints[JOINT_RIGHT_THIGH_LEG] = coneConstraint;
world->addConstraint(joints[JOINT_RIGHT_THIGH_LEG], true);
// RIGHT leg-foot
bodyA.setIdentity();
bodyA.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyA.setOrigin(btVector3(0, -(legHeight / 2 + legRadius), 0));
bodyB.setIdentity();
bodyB.getBasis().setEulerZYX(0, M_PI_2, 0);
bodyB.setOrigin(btVector3(-footXOffset, footHeight + 0.03, 0));
coneConstraint = new btConeTwistConstraint(*bodyParts[BODYPART_RIGHT_LEG]->getRigidBody(), *bodyParts[BODYPART_RIGHT_FOOT]->getRigidBody(), bodyA, bodyB);
coneConstraint->setLimit(M_PI_4, M_PI_4, 0);
joints[JOINT_RIGHT_LEG_FOOT] = coneConstraint;
world->addConstraint(joints[JOINT_RIGHT_LEG_FOOT], true);
}
//-------------------------------------------------------------------------------------
void OBTutorial2::createScene(void)
{
mCamera->setPosition(Vector3(0,18,70));
mCamera->lookAt(Vector3(0,0,-300));
mCamera->setNearClipDistance(5);
mSceneMgr->setAmbientLight(Ogre::ColourValue(0.9, 0.9, 0.9));
// Start Bullet
mWorld = new btDiscreteDynamicsWorld(mDispatcher, // gravity vector for Bullet
mBroadphase, mSolver, mCollisionConfiguration );
// add Debug info display tool
//debugDrawer = new OgreBulletCollisions::DebugDrawer();
//debugDrawer->setDrawWireframe(true); // we want to see the Bullet containers
//mWorld->setDebugDrawer(debugDrawer);
//mWorld->setShowDebugShapes(true); // enable it if you want to see the Bullet containers
//SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode("debugDrawer", Ogre::Vector3::ZERO);
//node->attachObject(static_cast <SimpleRenderable *> (debugDrawer));
// Define a floor plane mesh
Entity *ent;
Plane p;
p.normal = Vector3(0,1,0); p.d = 0;
MeshManager::getSingleton().createPlane("FloorPlane",
ResourceGroupManager::DEFAULT_RESOURCE_GROUP_NAME,
p, 200000, 200000, 20, 20, true, 1, 9000, 9000,
Vector3::UNIT_Z);
// Create an entity (the floor)
ent = mSceneMgr->createEntity("floor", "FloorPlane");
ent->setMaterialName("Examples/BumpyMetal");
mSceneMgr->getRootSceneNode()->createChildSceneNode()->attachObject(ent);
// add collision detection to it
btCollisionShape *Shape;
Shape = new btStaticPlaneShape(toBulletVector(Ogre::Vector3(0,1,0)), 0); // (normal vector, distance)
// a body is needed for the shape
MyMotionState * defaultMotionState = new MyMotionState(btTransform(btQuaternion(btScalar(0),btScalar(0),btScalar(0),btScalar(1))), ent->getParentSceneNode());
btRigidBody *defaultBody = new btRigidBody(btScalar(1), defaultMotionState, Shape);
btRigidBody *defaultPlaneBody = new btRigidBody(btScalar(0), defaultMotionState, Shape);
//defaultPlaneBody->setStaticShape(Shape, 0.1, 0.8);// (shape, restitution, friction)
// push the created objects to the deques
mShapes.push_back(Shape);
mBodies.push_back(defaultPlaneBody);
// create an cube mesh
Entity *entity = mSceneMgr->createEntity(
"Box" + StringConverter::toString(mNumEntitiesInstanced),
"cube.mesh");
entity->setMaterialName("Examples/BumpyMetal");
Ogre::SceneNode *node = mSceneMgr->getRootSceneNode()->createChildSceneNode();
node->attachObject(entity);
node->scale(0.05f, 0.05f, 0.05f); // the cube is too big for us
createBoxShape(entity, Ogre::Vector3(0.0,0.0,0.0), false);
}
void workshopScene::update(sceneInfo &info)
{
gWorld->btWorld->stepSimulation(1.f/60.f);
mousevelx = mousevelx * 0.95f + info.dmousex * 0.2f;
mousevely = mousevely * 0.95f + info.dmousey * 0.2f;
if (info.keys.held.MouseM)
{
camera.pitch -= info.dmousey * 0.02;
camera.yaw -= info.dmousex * 0.02;
camera.orientationFromAngles();
}
sceneInfo::keyState &keys = info.keys;
if (!glfwGetKey('E'))
{
if (keys.held.W)
{
camera.position += camera.forward * 0.1;
}
else if (keys.held.S)
{
camera.position -= camera.forward * 0.1;
}
if (keys.held.D)
{
camera.position += camera.right * 0.1;
}
else if (keys.held.A)
{
camera.position -= camera.right * 0.1;
}
}
if (info.captureMouse)
{
if (mouseWasCaptured)
{
cursorx += info.dmousex;
cursory += info.dmousey;
if (cursorx < 0)
cursorx = 0;
if (cursorx > info.width)
cursorx = info.width;
if (cursory < 0)
cursory = 0;
if (cursory > info.height)
cursory = info.height;
}
else
{
cursorx = info.lastmousex;
cursory = info.lastmousey;
}
}
else if (mouseWasCaptured)
{
glfwSetMousePos(cursorx, cursory);
}
mouseWasCaptured = info.captureMouse;
if (info.keys.held.space && selectedItem < partnames.size())
{
int initialcount = gWorld->objects.size();
loadAssembly(partnames[selectedItem], btTransform(btQuaternion(0, 0, 0, 1), btVector3(0, 10, 0)), path, gWorld);
for (unsigned int i = initialcount; i < gWorld->objects.size(); i++)
gWorld->objects[i]->body->setDamping(0.95f, 0.96f);
}
if (cursorx < UI_SIZE)
{
if (keys.newPress.MouseL)
{
selectedItem = (cursory - 4) / (UI_SIZE + 8);
selectedTool = -1;
}
}
else if (cursory > info.height - UI_SMALL - 16)
{
if (keys.newPress.MouseL)
{
selectedTool = (cursorx - UI_SIZE - 16 - 8) / (UI_SMALL + 8);
selectedItem = -1;
}
}
else
{
mouseRayDir = camera.forward + camera.right * (cursorx - info.width / 2) / (float)info.height * 2 - camera.up * (cursory - info.height / 2) / (float)info.height * 2;
btVector3 raystart = camera.position;
btVector3 rayend = raystart + mouseRayDir * 100;
mouseRayCallback = btCollisionWorld::ClosestRayResultCallback(raystart, rayend);
gWorld->btWorld->rayTest(raystart, rayend, mouseRayCallback);
if (keys.newPress.MouseL && mouseRayCallback.hasHit())
{
mouseHeldBody = (btRigidBody*)mouseRayCallback.m_collisionObject;
if (selectedItem >= 0)
{
if (selectedItem < partnames.size())
{
int initialcount = gWorld->objects.size();
btTransform trans(btQuaternion(0, 0, 0, 1), mouseRayCallback.m_hitPointWorld + mouseRayCallback.m_hitNormalWorld * 1.f);
loadAssembly(partnames[selectedItem], trans, path, gWorld);
for (unsigned int i = initialcount; i < gWorld->objects.size(); i++)
gWorld->objects[i]->body->setDamping(0.95f, 0.96f);
}
}
else if (selectedTool == 0)
{
mousePerpDist = camera.forward.dot(mouseRayCallback.m_hitPointWorld - raystart);
mouseHeldBody->setDamping(0.995, 0.98);
mouseHeldBody->activate();
btVector3 localPivot = mouseHeldBody->getCenterOfMassTransform().inverse() * mouseRayCallback.m_hitPointWorld;
mouseConstraint = new btGeneric6DofConstraint(*mouseHeldBody, btTransform(btQuaternion(0, 0, 0, 1), localPivot), false);
if (glfwGetKey('E'))
{
mouseConstraint->setAngularLowerLimit(btVector3(0, 0, 0));
mouseConstraint->setAngularUpperLimit(btVector3(0, 0, 0));
}
gWorld->btWorld->addConstraint(mouseConstraint);
}
else if (selectedTool == 1)
{
if (!mouseHeldBody->isStaticObject())
{
if (!axisHasFirst)
{
axisHasFirst = true;
axisResult = mouseRayCallback;
axisFirstPivot = mouseHeldBody->getCenterOfMassTransform().inverse() * axisResult.m_hitPointWorld;
axisFirstNormal = btTransform(mouseHeldBody->getCenterOfMassTransform().inverse().getRotation(), btVector3(0, 0, 0)) * axisResult.m_hitNormalWorld;
std::cout << "First point for axis.\n";
}
else
{
btVector3 axisSecondNormal = btTransform(mouseHeldBody->getCenterOfMassTransform().inverse().getRotation(), btVector3(0, 0, 0)) * mouseRayCallback.m_hitNormalWorld;
btVector3 axisSecondPivot = mouseHeldBody->getCenterOfMassTransform().inverse() * mouseRayCallback.m_hitPointWorld + axisSecondNormal * 0.05;
gWorld->addConstraint(new btHingeConstraint(*(btRigidBody*)axisResult.m_collisionObject, *mouseHeldBody, axisFirstPivot, axisSecondPivot, -axisFirstNormal, axisSecondNormal));
mouseHeldBody->activate();
axisHasFirst = false;
}
}
}
else if (selectedTool == 2)
{
btRigidBody *body = (btRigidBody*)mouseRayCallback.m_collisionObject;
if (!body->isStaticObject())
gWorld->removeBody(body);
}
}
}
if (mouseConstraint)
{
if (keys.held.MouseL)
{
mousePerpDist *= pow(1.1, keys.dmouseWheel);
mouseConstraint->getFrameOffsetA().setOrigin(camera.position + mouseRayDir * mousePerpDist); // note that raydir is not unit length: it stretches from the camera to the near plane.
}
else
{
mouseHeldBody->setDamping(0.95, 0.96);
gWorld->btWorld->removeConstraint(mouseConstraint);
delete mouseConstraint;
mouseConstraint = 0;
}
if (glfwGetKey('E'))
{
btTransform invrotate;
mouseHeldBody->getMotionState()->getWorldTransform(invrotate);
invrotate = invrotate.inverse();
if (keys.held.W)
mouseHeldBody->applyImpulse(invrotate * camera.forward, invrotate * camera.up);
if (keys.held.S)
mouseHeldBody->applyImpulse(-camera.forward, camera.up);
if (keys.held.A)
mouseHeldBody->applyImpulse(-camera.right, camera.forward);
if (keys.held.D)
mouseHeldBody->applyImpulse(camera.right, camera.forward);
}
}
if (axisHasFirst && selectedTool != 1)
axisHasFirst = false;
}
