glm::vec3 CarrierModelComponent::getSpawnPoint() const
{
// TODO: Clean up after transform is made better.
CaffComp::TransformComponent *transform = getOwner()->getTransform();
CaffComp::PhysicsComponent *physics = getOwner()->getRigidBody();
const btVector3 fwd = physics->getRigidBody()->getWorldTransform().getBasis().getColumn(2);
const glm::vec3 forward(fwd.x(), fwd.y(), fwd.z());
const glm::vec3 result = transform->getPosition() + (m_spawnPoint - forward);
return result;
}
void boundaryConstrain(int i)
{
btFluidParticles& particles = fluid->internalGetParticles();
btVector3 &velFluid = particles.m_vel[i];
btVector3 &posFluid = particles.m_pos[i];
collisionWithBound(velFluid, posFluid, btVector3( 1.0, 0.0, 0.0), posFluid.x() - minBound.x() - Constant.m_particleRadius);
collisionWithBound(velFluid, posFluid, btVector3( -1.0, 0.0, 0.0), maxBound.x() - posFluid.x() - Constant.m_particleRadius);
collisionWithBound(velFluid, posFluid, btVector3( 0.0, 1.0, 0.0), posFluid.y() - minBound.y() - Constant.m_particleRadius);
collisionWithBound(velFluid, posFluid, btVector3( 0.0, -1.0, 0.0), maxBound.y() - posFluid.y() - Constant.m_particleRadius);
collisionWithBound(velFluid, posFluid, btVector3( 0.0, 0.0, 1.0), posFluid.z() - minBound.z() - Constant.m_particleRadius);
collisionWithBound(velFluid, posFluid, btVector3( 0.0, 0.0, -1.0), maxBound.z() - posFluid.z() - Constant.m_particleRadius);
}
virtual void drawSpherePatch(const btVector3& center, const btVector3& up, const btVector3& axis, btScalar radius,
btScalar minTh, btScalar maxTh, btScalar minPs, btScalar maxPs, const btVector3& color, btScalar stepDegrees = btScalar(10.f))
{
double center_[3] = {center.x() / bulletWorldScalingFactor,
center.y() / bulletWorldScalingFactor,
center.z() / bulletWorldScalingFactor};
double up_[3] = {up.x() / bulletWorldScalingFactor,
up.y() / bulletWorldScalingFactor,
up.z() / bulletWorldScalingFactor};
double axis_[3] = {axis.x() / bulletWorldScalingFactor,
axis.y() / bulletWorldScalingFactor,
axis.z() / bulletWorldScalingFactor};
double color_[3] = {color.x(), color.y(), color.z()};
if(callbacks_.drawSpherePatch)
(*callbacks_.drawSpherePatch)(center_, up_, axis_, radius / bulletWorldScalingFactor,
minTh, maxTh, minPs, maxPs, color_, stepDegrees, arg_);
else
btIDebugDraw::drawSpherePatch(center / bulletWorldScalingFactor, up / bulletWorldScalingFactor,
axis / bulletWorldScalingFactor, radius / bulletWorldScalingFactor,
minTh, maxTh, minPs, maxPs, color, stepDegrees);
}
// the center of editing takes place at HERE
// raise terrain DIR = 1, lower terrain DIR = -1
void terrain::terrainEdit(btVector3 here, int dir)
{
float area = tTool->diameter();
float amount = tTool->increment();
if(area <= 0 || amount <= 0) return;
amount *= dir; // determine the direction of editing, up or down
float xp = here.x(); // make sure the X location of the edit is over the terrain
if(xp < 0) xp = 0;
else if(xp > m_terrainSize.x()) xp = m_terrainSize.x();
float yp = here.y(); // make sure the Y location of the edit is over the terrain
if(yp < 0) yp = 0;
else if(yp > m_terrainSize.y()) yp = m_terrainSize.y();
for(int i=0; i<m_terrainVertexCount; i++) // loop through the terrain vertex array
{
float x = (i % m_pixelSize.width()) * m_terrainSize.x()/m_pixelSize.width();// get the X and Y incides of the terrain vertices
float y = (i / m_pixelSize.width()) * m_terrainSize.y()/m_pixelSize.height();
float xd = xp - x;
float yd = yp - y;
float d = (float)sqrt((xd*xd) + (yd*yd)); // calculate the diameter of the area
float a = (m_terrainVerts[i].z + amount) - (amount*(d / area)); // calculate the new height of the vertex
if((dir == 1 && a > m_terrainVerts[i].z) || (dir == -1 && a < m_terrainVerts[i].z)) {
m_terrainVerts[i].z = a;
if(a > m_terrainMaxHeight)
m_terrainMaxHeight = a;
else if(a < m_terrainMinHeight)
m_terrainMinHeight = a;
float avgheight = (m_terrainMaxHeight + m_terrainMinHeight)/2.0;
if(a > avgheight) m_terrainColors[i].x = 0.8; // set red color
else m_terrainColors[i].x = 0.;
m_terrainColors[i].y = 0.; // set green color
if(a < avgheight) m_terrainColors[i].z = 0.8; // set blue color
else m_terrainColors[i].z = 0.;
}
}
buildNormals();
this->terrainRefresh();
m_terrainModified = true;
}
virtual void drawArc(const btVector3& center, const btVector3& normal, const btVector3& axis, btScalar radiusA, btScalar radiusB, btScalar minAngle, btScalar maxAngle,
const btVector3& color, bool drawSect, btScalar stepDegrees = btScalar(10.f))
{
double center_[3] = {center.x() / bulletWorldScalingFactor,
center.y() / bulletWorldScalingFactor,
center.z() / bulletWorldScalingFactor};
double normal_[3] = {normal.x() / bulletWorldScalingFactor,
normal.y() / bulletWorldScalingFactor,
normal.z() / bulletWorldScalingFactor};
double axis_[3] = {axis.x() / bulletWorldScalingFactor,
axis.y() / bulletWorldScalingFactor,
axis.z() / bulletWorldScalingFactor};
double color_[3] = {color.x(), color.y(), color.z()};
if(callbacks_.drawArc)
(*callbacks_.drawArc)(center_, normal_, axis_,
radiusA / bulletWorldScalingFactor, radiusB / bulletWorldScalingFactor,
minAngle, maxAngle, color_, drawSect, stepDegrees, arg_);
else
btIDebugDraw::drawArc(center / bulletWorldScalingFactor, normal / bulletWorldScalingFactor,
axis / bulletWorldScalingFactor, radiusA / bulletWorldScalingFactor, radiusB / bulletWorldScalingFactor,
minAngle, maxAngle, color, drawSect, stepDegrees);
}
void collider::on_collision(
collision_controller *other,
btVector3 point,
btVector3 normal,
btScalar depth)
{
push_component();
lua_getfield(L, -1, "on_collision");
if(lua_isnil(L, -1))
lua_pop(L, 1);
else
{
collider *other_collider = dynamic_cast<collider*>(other);
if(other_collider)
other_collider->push_component();
else
lua_pushboolean(L, 0);
l_pushvect(L, point.x(), point.y(), point.z());
l_pushvect(L, normal.x(), normal.y(), normal.z());
lua_pushnumber(L, depth);
lua_call(L, 4, 0);
}
lua_pop(L, 1);
}
geometry_msgs::TransformStamped createTransform(btQuaternion q, btVector3 v, ros::Time stamp, const std::string& frame1, const std::string& frame2)
{
geometry_msgs::TransformStamped t;
t.header.frame_id = frame1;
t.child_frame_id = frame2;
t.header.stamp = stamp;
t.transform.translation.x = v.x();
t.transform.translation.y = v.y();
t.transform.translation.z = v.z();
t.transform.rotation.x = q.x();
t.transform.rotation.y = q.y();
t.transform.rotation.z = q.z();
t.transform.rotation.w = q.w();
return t;
}
void btWorldFactory::createBoxesFloor(QVariantList &shapesList, double areaX, double areaZ, btVector3 pos, btVector3 boxMin, btVector3 boxMax) {
// Boxes Floor
double sizeX = Tools::random((double)boxMin.x(), (double)boxMax.x());
double sizeZ = Tools::random((double)boxMin.z(), (double)boxMax.z());
btVector3 recal(sizeX,0,sizeZ);
pos = pos + recal;
int nbBoxesX = areaX / sizeX;
int nbBoxesZ = areaZ / sizeZ;
for(int i=0;i<nbBoxesX;++i){
for(int j=0;j<nbBoxesZ;++j){
double sizeY = Tools::random((double)boxMin.y(), (double)boxMax.y());
createBox(shapesList,
btVector3(sizeX,sizeY,sizeZ),
btVector3(i*sizeX - areaX*0.5 + pos.x(),sizeY/2.0 + pos.y(),j*sizeZ - areaZ*0.5 + pos.z()),
btVector3(0,0,0),0);
}
}
}
/**
* A cloud of smoke
**/
void EffectsManager::smokeCloud(const btVector3& location)
{
SPK::Emitter* emitter = SPK::NormalEmitter::create();
emitter->setZone(SPK::Sphere::create(SPK::Vector3D(location.x(), location.y(), location.z()), 2.0f));
emitter->setFlow(-1);
emitter->setTank(50);
emitter->setForce(3.0f, 5.0f);
SPK::Group* group = SPK::Group::create(model_smoke, 50);
group->addEmitter(emitter);
group->setGravity(gravity);
group->setFriction(1.0f);
group->setRenderer(render_smoke);
st->addParticleGroup(group);
}
CellPosition Grid::getCellPosition(const btVector3& position) const
{
int k1 = int((position.x() - m_minBoundary.x()) / m_gridCellSize);
if (k1 < 0) k1 = 0;
else if (k1 >= m_data.size()) k1 = m_data.size()-1;
int k2 = int((position.y() - m_minBoundary.y()) / m_gridCellSize);
if (k2 < 0) k2 = 0;
else if (k2 >= m_data[0].size()) k2 = m_data[0].size()-1;
int k3 = int((position.z() - m_minBoundary.z()) / m_gridCellSize);
if (k3 < 0) k3 = 0;
else if (k3 >= m_data[0][0].size()) k3 = m_data[0][0].size()-1;
return CellPosition(k1, k2, k3);
}
void DrawTriangle(const btVector3& p0, const btVector3& p1, const btVector3& p2, const btVector3& color)
{
// glDisable(GL_LIGHTING);
glColor4f(color.x(), color.y(), color.z(), 1.0f);
btVector3 tmp[] = {p0, p1, p2};
glEnableClientState(GL_VERTEX_ARRAY);
#ifndef BT_USE_DOUBLE_PRECISION
glVertexPointer(3, GL_FLOAT, sizeof(btVector3), &tmp[0].x());
#else
glVertexPointer(3, GL_DOUBLE, sizeof(btVector3), &tmp[0].x());
#endif
glDrawArrays(GL_TRIANGLES, 0, 3);
glDisableClientState(GL_VERTEX_ARRAY);
// glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
// glEnable(GL_LIGHTING);
}
/**
* Spray blood in all directions
**/
void EffectsManager::bloodSpray(const btVector3& location, float damage)
{
SPK::RandomEmitter* emitter;
SPK::Group* group;
emitter = SPK::RandomEmitter::create();
emitter->setZone(SPK::Point::create(SPK::Vector3D(location.x(), location.y() - 0.5f, location.z())));
emitter->setFlow(-1);
emitter->setTank(damage / 10.0f);
emitter->setForce(5.0f, 10.0f);
group = SPK::Group::create(model_blood, damage);
group->addEmitter(emitter);
group->setGravity(gravity);
group->setRenderer(render_blood);
st->addParticleGroup(group);
}
static void DrawLine(const btVector3& p0, const btVector3& p1, const btVector3& color, float line_width)
{
glDisable(GL_LIGHTING);
glLineWidth(line_width);
glColor4f(color.x(), color.y(), color.z(), 1.0f);
btVector3 tmp[] = {p0, p1};
glEnableClientState(GL_VERTEX_ARRAY);
#ifndef BT_USE_DOUBLE_PRECISION
glVertexPointer(3, GL_FLOAT, sizeof(btVector3), &tmp[0].x());
#else
glVertexPointer(3, GL_DOUBLE, sizeof(btVector3), &tmp[0].x());
#endif
glDrawArrays(GL_LINES, 0, 2);
glDisableClientState(GL_VERTEX_ARRAY);
glColor4f(1.0f, 1.0f, 1.0f, 1.0f);
glEnable(GL_LIGHTING);
}
btMatrix3x3 GetOrientedBasis(btVector3 const &z)
{
btAssert(fabsf(z.length()-1) < 0.01f);
btVector3 t(0,0,0);
if(fabsf(z.z() < 0.999f))
{
t.setZ(1);
}
else
{
t.setX(1);
}
btVector3 x = t.cross(z).normalize();
btVector3 y = z.cross(x).normalize();
return btMatrix3x3( x.x(), y.x(), z.x(),
x.y(), y.y(), z.y(),
x.z(), y.z(), z.z());
}
void TrackDisplay::createLine(Ogre::Vector3 pos, Ogre::Vector3 old_pos,
Ogre::Vector3 scale, btVector3 color, std::vector<
ogre_tools::BillboardLine*> &vec,bool add_cross) {
ogre_tools::BillboardLine* lines = newBillboardLine();
lines->setPosition(pos);
lines->setOrientation(Ogre::Quaternion(1,0,0,0));
lines->setScale(scale);
lines->setColor(color.x(), color.y(), color.z(), alpha_);
lines->clear();
lines->setLineWidth(lineWidth_);
lines->setMaxPointsPerLine(2);
lines->setNumLines(4);
Ogre::Vector3 v;
if(add_cross) {
for(int i=0;i<3;i++) {
v = Ogre::Vector3(0, 0, 0);
v[i] = lineWidth_ * 5;
rviz::robotToOgre(v);
lines->addPoint(v);
v = Ogre::Vector3(0, 0, 0);
v[i] = -lineWidth_ * 5;
rviz::robotToOgre(v);
lines->addPoint(v);
lines->newLine();
}
}
v = Ogre::Vector3(0, 0, 0);
rviz::robotToOgre(v);
lines->addPoint(v);
v = old_pos - pos;
// rviz::robotToOgre(v);
lines->addPoint(v);
vec.push_back(lines);
}
bool TrackDisplay::transform(const btTransform &pose, const btVector3 &scaleIn,
Ogre::Vector3& pos, Ogre::Quaternion& orient, Ogre::Vector3& scaleOut) {
pos = Ogre::Vector3(pose.getOrigin().x(), pose.getOrigin().y(),
pose.getOrigin().z());
rviz::robotToOgre(pos);
btQuaternion quat;
pose.getBasis().getRotation(quat);
orient = Ogre::Quaternion::IDENTITY;
rviz::ogreToRobot(orient);
orient = Ogre::Quaternion(quat.w(), quat.x(), quat.y(), quat.z()) * orient;
rviz::robotToOgre(orient);
scaleOut = Ogre::Vector3(scaleIn.x(), scaleIn.y(), scaleIn.z());
rviz::scaleRobotToOgre(scaleOut);
return true;
}
virtual void flushLines()
{
int sz = m_linePoints.size();
if (sz)
{
float debugColor[4];
debugColor[0] = m_currentLineColor.x();
debugColor[1] = m_currentLineColor.y();
debugColor[2] = m_currentLineColor.z();
debugColor[3] = 1.f;
m_glApp->m_renderer->drawLines(&m_linePoints[0].x,debugColor,
m_linePoints.size(),sizeof(MyDebugVec3),
&m_lineIndices[0],
m_lineIndices.size(),
1);
m_linePoints.clear();
m_lineIndices.clear();
}
}
NxQuat(const float angle, const btVector3 & axis)
{
x = axis.x();
y = axis.y();
z = axis.z();
const float i_length = 1.0f / sqrtf( x*x + y*y + z*z );
x = x * i_length;
y = y * i_length;
z = z * i_length;
float Half = degToRad(angle * 0.5f);
w = cosf(Half);
const float sin_theta_over_two = sinf(Half );
x = x * sin_theta_over_two;
y = y * sin_theta_over_two;
z = z * sin_theta_over_two;
}
void GameObjectStaticManager::CreateEntity(char * path, PhysicsEngine* engine, btQuaternion& orientation,
btVector3& position, btScalar mass)
{
std::string pathStr(path);
string base = pathStr.erase(pathStr.find_last_of('/'));
base.push_back('/');
// Set object position
glm::mat4x4 modelMatrix = mat4_cast(quat(orientation.w(), orientation.x(), orientation.y(), orientation.z()));
modelMatrix = glm::translate(modelMatrix, glm::vec3(position.x(), position.y(), position.z()));
// Get the mode pair
std::pair<int, Model*>modelPair = resource::ModelManager::GetSingleton()->GetModel(modelMatrix, path);
// If the first entry of the pair is -1 then the model has not yet been loaded in.
if (modelPair.first == -1)
{
// Load the obj files
std::vector<tinyobj::shape_t> shapes;
std::vector<tinyobj::material_t> materials;
tinyobj::LoadObj(shapes, materials, path, base.c_str());
StaticPhysicsObject* physStart = engine->CreateStaticObject(shapes, orientation, position,
btVector3(1, 1, 1), mass, path);
Model* modelStart = resource::ModelManager::GetSingleton()->CreateModels(modelMatrix, shapes, materials, base.c_str(), path);
new (&m_gameEntity[m_activeEntitys]) GameEntity(m_activeEntitys, shapes.size(), modelStart, physStart);
m_activeEntitys++;
}
else
{
std::pair<int, StaticPhysicsObject*>physPair = engine->GetBody(path, orientation, position, btVector3(1, 1, 1), mass);
new (&m_gameEntity[m_activeEntitys]) GameEntity(m_activeEntitys, modelPair.first, modelPair.second, physPair.second);
m_activeEntitys++;
}
#ifdef BUILDER_MODE
std::string temp = GetName(path);
temp.push_back('_' + m_entityMap.size());
m_entityMap.insert(std::pair<string, std::pair<std::string, GameEntity*>>(temp, std::pair<std::string, GameEntity*>(path,&m_gameEntity[m_activeEntitys - 1])));
#endif
}
btVector3 btBarrelShape::localGetSupportingVertexWithoutMargin(const btVector3& vec0)const
{
btVector3 supVec(0,0,0);
btVector3 supVecD;
// suppport point on the lathed ellipse?
btScalar pY = vec0.y();
btScalar pR = std::sqrt (vec0.z()*vec0.z() + vec0.x()*vec0.x() );
btScalar pH = pR;
btScalar dX = vec0.x()/pR;
btScalar dZ = vec0.z()/pR;
btScalar tpar = std::atan((pY*R_vert)/(pH*R_hor));
btScalar sY = R_vert * sin(tpar);
btScalar sH = R_hor * cos(tpar);
btScalar sR = sH + R_offset;
btScalar sX = dX * sR;
btScalar sZ = dZ * sR;
supVec.setValue(sX,sY,sZ);
btScalar len = supVec.length();
// support point on the top disc?
if ((fabs(Y_high) < R_vert)&(supVec.y()>Y_high))
{
btScalar R_high_ellips = R_hor * sqrt( 1- pow( Y_high/R_vert ,2) );
btScalar R_high = R_high_ellips + R_offset;
btScalar rad_ratio = pR/R_high;
supVecD.setValue(vec0.x()/rad_ratio, Y_high, vec0.z()/rad_ratio);
supVec = supVecD;
}
// support point on the bottom disc?
if ((fabs(Y_low) < R_vert)&(supVec.y()<Y_low))
{
btScalar R_low_ellips = R_hor * sqrt( 1- pow( Y_low/R_vert ,2) );
btScalar R_low = R_low_ellips + R_offset;
btScalar rad_ratio = pR/R_low;
supVecD.setValue(vec0.x()/rad_ratio, Y_low, vec0.z()/rad_ratio);
supVec = supVecD;
}
return supVec;
}
// tries out a grasp and returns the number of anchors attached
static int tryGrasp(const GraspingActionContext &ctx, int node, const btVector3 &gripperdir) {
GraspingActionContext forkctx = ctx.fork();
int startNumAnchors = ctx.cloth->softBody->m_anchors.size();
stringstream ss; ss << "grab " << node << ' ' << gripperdir.x() << ' ' << gripperdir.y() << ' ' << gripperdir.z();
try {
Action::Ptr a = GraspingActionSpec(ss.str()).createAction(forkctx);
while (!a->done()) {
a->step(BulletConfig::dt);
forkctx.env->step(BulletConfig::dt,
BulletConfig::maxSubSteps, BulletConfig::internalTimeStep);
}
} catch (const GraspingActionFailed &) {
return 0;
}
return forkctx.cloth->softBody->m_anchors.size() - startNumAnchors;
}
/**
* It's an EXPLOSION!
**/
void EffectsManager::explosion(const btVector3& location, float damage)
{
SPK::Emitter* emitter;
SPK::Group* group;
// Emitter
emitter = SPK::RandomEmitter::create();
emitter->setZone(SPK::Point::create(SPK::Vector3D(location.x(), location.y(), location.z())));
emitter->setFlow(-1);
emitter->setTank(100);
emitter->setForce(10.0f, 20.0f);
// Create group
group = SPK::Group::create(model_fireball, 100);
group->addEmitter(emitter);
group->setGravity(gravity);
group->setRenderer(render_fireball);
st->addParticleGroup(group);
this->smokeCloud(location);
}
void BulletCHOP::addBody(btVector3 pos, btVector3 rot, btVector3 scale, btScalar mass){
btCollisionShape* colShape = new btBoxShape(0.5*scale);
collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
btMatrix3x3 rotMat;
rotMat.setEulerZYX(rot.x(),rot.y(),rot.z());
startTransform.setOrigin(pos);
startTransform.setBasis(rotMat);
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
if(isDynamic == 0){
body->setCollisionFlags(body->getCollisionFlags() | btCollisionObject::CF_KINEMATIC_OBJECT);
body->setActivationState(DISABLE_DEACTIVATION);
}
dynamicsWorld->addRigidBody(body);
}
/////////////////////////////////////////
// UTILITY FUNCTIONS
/////////////
// returns TRUE if pt is inside of the CONVEX polygon LS, in world coordinates
bool cSpace::isPointInsidePoly(btVector3 pt,QList<btVector3> ls)
{
int i,iplus;
double z;
btVector3 p1, p2;
// traveling around the polygon point list in a CCW (right hand rule) direction
// if z is negative for all sides, pt is inside of convex polygon ls
for(i=0; i<ls.size(); i++)
{
if(i == ls.size()-1) iplus = 0;
else iplus = i+1;
p1 = ls[i];
p2 = ls[iplus];
// pt is on the right side z is positive, left is negative
z = (pt.x() - p1.x())*(p2.y() - p1.y()) - (pt.y() - p1.y())*(p2.x() - p1.x());
if(z > 0) return false; // if the point is to the right of any side pt is outside the polygon
}
return true;
}
void TrackDisplay::createRectangle(Ogre::Vector3 pos, Ogre::Quaternion orient,
Ogre::Vector3 scale, double w, double h, btVector3 color, std::vector<
ogre_tools::BillboardLine*> &vec) {
ogre_tools::BillboardLine* lines = newBillboardLine();
lines->setPosition(pos);
lines->setOrientation(orient);
lines->setScale(scale);
lines->setColor(color.x(), color.y(), color.z(), alpha_);
lines->clear();
lines->setLineWidth(lineWidth_);
lines->setMaxPointsPerLine(5);
lines->setNumLines(1);
Ogre::Vector3 v;
v = Ogre::Vector3(0, 0, 0);
rviz::robotToOgre(v);
lines->addPoint(v);
v = Ogre::Vector3(w, 0, 0);
rviz::robotToOgre(v);
lines->addPoint(v);
v = Ogre::Vector3(w, h, 0);
rviz::robotToOgre(v);
lines->addPoint(v);
v = Ogre::Vector3(0, h, 0);
rviz::robotToOgre(v);
lines->addPoint(v);
v = Ogre::Vector3(0, 0, 0);
rviz::robotToOgre(v);
lines->addPoint(v);
vec.push_back(lines);
}
void BtDebugDraw::drawLine( const btVector3 &fromBt, const btVector3 &toBt, const btVector3 &color )
{
Point3F from = btCast<Point3F>( fromBt );
Point3F to = btCast<Point3F>( toBt );
// Cull first if we have a frustum.
//F32 distSquared = ( mCuller->getPosition() - from ).lenSquared();
//if ( mCuller && distSquared > ( 150 * 150 ) ) //!mCuller->clipSegment( from, to ) )
//return;
// Do we need to flush the builder?
if ( mVertexCount + 2 >= 1000 )
flush();
// Are we starting a new primitive?
if ( mVertexCount == 0 )
PrimBuild::begin( GFXLineList, 1000 );
PrimBuild::color3f( color.x(), color.y(), color.z() );
PrimBuild::vertex3f( from.x, from.y, from.z );
PrimBuild::vertex3f( to.x, to.y, to.z );
mVertexCount += 2;
}
void PhysicsWorld::drawLine(const btVector3& from, const btVector3& to, const btVector3& color)
{
if (debugRenderer_)
debugRenderer_->AddLine(ToVector3(from), ToVector3(to), Color(color.x(), color.y(), color.z()), debugDepthTest_);
}
void HfFluidDemo_GL_ShapeDrawer::drawOpenGL(btScalar* m, const btCollisionShape* shape, const btVector3& color,int debugMode,const btVector3& worldBoundsMin,const btVector3& worldBoundsMax)
{
glPushMatrix();
btglMultMatrix(m);
if (shape->getShapeType() == UNIFORM_SCALING_SHAPE_PROXYTYPE)
{
const btUniformScalingShape* scalingShape = static_cast<const btUniformScalingShape*>(shape);
const btConvexShape* convexShape = scalingShape->getChildShape();
float scalingFactor = (float)scalingShape->getUniformScalingFactor();
{
btScalar tmpScaling[4][4]={{scalingFactor,0,0,0},
{0,scalingFactor,0,0},
{0,0,scalingFactor,0},
{0,0,0,1}};
drawOpenGL( (btScalar*)tmpScaling,convexShape,color,debugMode,worldBoundsMin,worldBoundsMax);
}
glPopMatrix();
return;
}
if (shape->getShapeType() == HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE)
{
btConvexShape* convexShape = ((btHfFluidBuoyantConvexShape*)shape)->getConvexShape();
btTransform I;
I.setIdentity();
btScalar mat[16];
I.getOpenGLMatrix (&mat[0]);
drawOpenGL (mat, convexShape, color, debugMode, worldBoundsMin, worldBoundsMax);
return;
}
if (shape->getShapeType() == COMPOUND_SHAPE_PROXYTYPE)
{
const btCompoundShape* compoundShape = static_cast<const btCompoundShape*>(shape);
for (int i=compoundShape->getNumChildShapes()-1;i>=0;i--)
{
btTransform childTrans = compoundShape->getChildTransform(i);
const btCollisionShape* colShape = compoundShape->getChildShape(i);
btScalar childMat[16];
childTrans.getOpenGLMatrix(childMat);
drawOpenGL(childMat,colShape,color,debugMode,worldBoundsMin,worldBoundsMax);
}
} else
{
if(m_textureenabled&&(!m_textureinitialized))
{
GLubyte* image=new GLubyte[256*256*3];
for(int y=0;y<256;++y)
{
const int t=y>>4;
GLubyte* pi=image+y*256*3;
for(int x=0;x<256;++x)
{
const int s=x>>4;
const GLubyte b=180;
GLubyte c=b+((s+t&1)&1)*(255-b);
pi[0]=pi[1]=pi[2]=c;pi+=3;
}
}
glGenTextures(1,(GLuint*)&m_texturehandle);
glBindTexture(GL_TEXTURE_2D,m_texturehandle);
glTexEnvf(GL_TEXTURE_ENV,GL_TEXTURE_ENV_MODE,GL_MODULATE);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MIN_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_MAG_FILTER,GL_LINEAR_MIPMAP_LINEAR);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_S,GL_REPEAT);
glTexParameterf(GL_TEXTURE_2D,GL_TEXTURE_WRAP_T,GL_REPEAT);
gluBuild2DMipmaps(GL_TEXTURE_2D,3,256,256,GL_RGB,GL_UNSIGNED_BYTE,image);
delete[] image;
glMatrixMode(GL_TEXTURE);
glLoadIdentity();
glScalef(0.025,0.025,0.025);
static const GLfloat planex[]={1,0,0,0};
static const GLfloat planey[]={0,1,0,0};
static const GLfloat planez[]={0,0,1,0};
glTexGenfv(GL_S,GL_OBJECT_PLANE,planex);
glTexGenfv(GL_T,GL_OBJECT_PLANE,planez);
glTexGeni(GL_S,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR);
glTexGeni(GL_T,GL_TEXTURE_GEN_MODE,GL_OBJECT_LINEAR);
glEnable(GL_TEXTURE_GEN_S);
glEnable(GL_TEXTURE_GEN_T);
glEnable(GL_TEXTURE_GEN_R);
m_textureinitialized=true;
}
//drawCoordSystem();
//glPushMatrix();
glEnable(GL_COLOR_MATERIAL);
if(m_textureenabled)
{
glEnable(GL_TEXTURE_2D);
glBindTexture(GL_TEXTURE_2D,m_texturehandle);
} else
{
glDisable(GL_TEXTURE_2D);
}
glColor3f(color.x(),color.y(), color.z());
bool useWireframeFallback = true;
if (!(debugMode & btIDebugDraw::DBG_DrawWireframe))
{
static void RenderCallback()
{
// Clear buffers
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
// Setup camera
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
gluPerspective(60.0f, ((float)glutGet(GLUT_WINDOW_WIDTH))/((float)glutGet(GLUT_WINDOW_HEIGHT)), 1.0f, 10000.0f);
gluLookAt(Eye.x(), Eye.y(), Eye.z(), Eye.x() + Dir.x(), Eye.y() + Dir.y(), Eye.z() + Dir.z(), 0.0f, 1.0f, 0.0f);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glEnable(GL_LIGHTING);
//clear previous frames result
gNormal.setValue(10,0,0);
gPoint.setValue(0,0,0);
gDepth = 999.999;
gLastUsedMethod = -1;
gNumGjkIterations = -1;
TestEPA(gConvex0, gConvex1);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
btVector3 RefSep(btScalar(0.), btScalar(0.), btScalar(0.));
float RefDMin=0.f;
bool RefResult = false;
if(gRefMode)
RefResult = ReferenceCode(gConvex0, gConvex1, RefDMin, RefSep);
// DrawLine(gPoint, gPoint + gNormal*20.0f, btVector3(1,0,0), 2.0f);
// printf("%f: %f %f %f\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());
#ifdef VERBOSE_TEXT_ONSCREEN
glColor3f(255.f, 255.f, 255.f);
setOrthographicProjection();
float xOffset = 10.f;
float yStart = 20.f;
float yIncr = 20.f;
char buf[124];
sprintf(buf,"gDepth=%f: gNormal=(%f %f %f)\n", gDepth, gNormal.x(), gNormal.y(), gNormal.z());
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"num GJK iterations =%d\n", gNumGjkIterations);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
sprintf(buf,"gLastUsedMethod=%d\n", gLastUsedMethod);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
if (gLastUsedMethod >= 3)
{
switch ( gMethod)
{
case 0:
sprintf(buf,"Bullet GjkEpa Penetration depth solver (zlib free\n" );
break;
case 1:
sprintf(buf,"Bullet Minkowski sampling Penetration depth solver\n" );
break;
case 2:
sprintf(buf,"Solid35 EPA Penetration depth solver\n" );
break;
case 3:
sprintf(buf,"EPA Penetration depth solver (Experimental/WorkInProgress, zlib free\n" );
break;
default:
sprintf(buf,"Unknown Penetration Depth\n" );
}
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
} else
{
sprintf(buf,"Hybrid GJK method %d\n", gLastUsedMethod);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
}
if (gLastDegenerateSimplex)
{
sprintf(buf,"DegenerateSimplex %d\n", gLastDegenerateSimplex);
GLDebugDrawString(xOffset,yStart,buf);
yStart += yIncr;
}
resetPerspectiveProjection();
#endif //VERBOSE_TEXT_ONSCREEN
btVector3 color(0,0,0);
gConvex0.Render(false, color);
gConvex1.Render(false, color);
if(gDepth<0.0f)
{
btTransform Saved = gConvex0.mTransform;
gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() - btVector3(gNormal*gDepth));
gConvex0.Render(true, btVector3(1.0f, 0.5f, 0.0f));
gConvex0.mTransform = Saved;
}
else
{
DrawLine(gPoint, gPoint + gNormal, btVector3(0,1,0), 2.0f);
}
if(RefResult & gRefMode)
{
btTransform Saved = gConvex0.mTransform;
gConvex0.mTransform.setOrigin(gConvex0.mTransform.getOrigin() + btVector3(RefSep*RefDMin));
gConvex0.Render(true, btVector3(0.0f, 0.5f, 1.0f));
gConvex0.mTransform = Saved;
}
glutSwapBuffers();
}
inline bool IsAlmostZero(const btVector3& v)
{
if(fabsf(v.x())>1e-6 || fabsf(v.y())>1e-6 || fabsf(v.z())>1e-6) return false;
return true;
}