btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
btVector3& axisInA,btVector3& axisInB)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
m_enableAngularMotor(false)
{
m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2;
btScalar projection = axisInA.dot(rbAxisA1);
if (projection >= 1.0f - SIMD_EPSILON) {
rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else if (projection <= -1.0f + SIMD_EPSILON) {
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else {
rbAxisA2 = axisInA.cross(rbAxisA1);
rbAxisA1 = rbAxisA2.cross(axisInA);
}
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),-axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),-axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),-axisInB.getZ() );
//start with free
m_lowerLimit = btScalar(1e30);
m_upperLimit = btScalar(-1e30);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
}
void drawLine(const btVector3& from,const btVector3& to,const btVector3& fromcolor, const btVector3& tocolor ) {
v[0] = from.getX();
v[1] = from.getY();
v[2] = from.getZ();
v[3] = to.getX();
v[4] = to.getY();
v[5] = to.getZ();
c[0] = fromcolor.getX();
c[1] = fromcolor.getY();
c[2] = fromcolor.getZ();
c[3] = tocolor.getX();
c[4] = tocolor.getY();
c[5] = tocolor.getZ();
glEnableClientState(GL_VERTEX_ARRAY);
glVertexPointer(3, GL_FLOAT, 0, v);
glDrawArrays(GL_LINES, 0, 2);
}
// -----------------------------------------------------------------------------
void IrrDebugDrawer::drawLine(const btVector3& from, const btVector3& to,
const btVector3& color)
{
video::SColor c(255, (int)(color.getX()*255), (int)(color.getY()*255),
(int)(color.getZ()*255) );
irr_driver->getVideoDriver()->draw3DLine((const core::vector3df&)from,
(const core::vector3df&)to, c);
}
void OgreDebugDrawer::drawLine( const btVector3 &from, const btVector3 &to, const btVector3 &color )
{
ColourValue c( color.getX(), color.getY(), color.getZ() );
c.saturate();
mLines->position( cvt(from) );
mLines->colour( c );
mLines->position( cvt(to) );
mLines->colour( c );
}
void
PhysicsDebugDrawer::drawLine
(const btVector3& from,const btVector3& to,const btVector3& color, const btVector3& color2)
{
// Too many lines to be effective?
/*
BoundingBox bb;
bb.minimum.x = std::min(from.x(),to.x());
bb.minimum.y = std::min(from.y(),to.y());
bb.minimum.z = std::min(from.z(),to.z());
bb.maximum.x = std::max(from.x(),to.x());
bb.maximum.y = std::max(from.y(),to.y());
bb.maximum.z = std::max(from.z(),to.z());
if (mCamera->inFrustum(bb))
{
*/
PhysicsDebugVertex a, b;
a.Position.x = from.getX();
a.Position.y = from.getY();
a.Position.z = from.getZ();
a.Color.r = color.getX();
a.Color.g = color.getY();
a.Color.b = color.getZ();
mVertexBuffer.push_back(a);
b.Position.x = to.getX();
b.Position.y = to.getY();
b.Position.z = to.getZ();
b.Color.r = color2.getX();
b.Color.g = color2.getY();
b.Color.b = color2.getZ();
mVertexBuffer.push_back(b);
/*
}
else
{
auto log = getLog();
log->error("Skipping debug point");
}
*/
}
// -----------------------------------------------------------------------------
void IrrDebugDrawer::drawLine(const btVector3& from, const btVector3& to,
const btVector3& color)
{
video::SColor c(255, (int)(color.getX()*255), (int)(color.getY()*255),
(int)(color.getZ()*255) );
//World::getWorld()->getCa
if (from.distance2(m_camera_pos) > 10000) return;
std::vector<float>& v = m_lines[c];
v.push_back(from.getX());
v.push_back(from.getY());
v.push_back(from.getZ());
v.push_back(to.getX());
v.push_back(to.getY());
v.push_back(to.getZ());
//draw3DLine((const core::vector3df&)from, (const core::vector3df&)to, c);
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,btRigidBody& rbB, const btVector3& pivotInA,const btVector3& pivotInB,
const btVector3& axisInA,const btVector3& axisInB, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA,rbB),
m_angularOnly(false),
m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0),m_limit()
{
m_rbAFrame.getOrigin() = pivotInA;
// since no frame is given, assume this to be zero angle and just pick rb transform axis
btVector3 rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(0);
btVector3 rbAxisA2;
btScalar projection = axisInA.dot(rbAxisA1);
if (projection >= 1.0f - SIMD_EPSILON) {
rbAxisA1 = -rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else if (projection <= -1.0f + SIMD_EPSILON) {
rbAxisA1 = rbA.getCenterOfMassTransform().getBasis().getColumn(2);
rbAxisA2 = rbA.getCenterOfMassTransform().getBasis().getColumn(1);
} else {
rbAxisA2 = axisInA.cross(rbAxisA1);
rbAxisA1 = rbAxisA2.cross(axisInA);
}
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = pivotInB;
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
void OgreBtDebugDrawer::drawLine(const btVector3 &from, const btVector3 &to,
const btVector3 &color) {
if (color == btVector3(0, 0, 0)) {
drawLine(OgreBulletUtils::convert(from), OgreBulletUtils::convert(to),
Ogre::ColourValue(1, 1, 1));
} else {
drawLine(OgreBulletUtils::convert(from), OgreBulletUtils::convert(to),
Ogre::ColourValue(color.getX(), color.getY(), color.getZ()));
}
}
/** Interpolates the normal at the given position for the triangle with
* a given index. The position must be inside of the given triangle.
* \param index Index of the triangle to use.
* \param position The position for which to interpolate the normal.
*/
btVector3 TriangleMesh::getInterpolatedNormal(unsigned int index,
const btVector3 &position) const
{
btVector3 p1, p2, p3;
getTriangle(index, &p1, &p2, &p3);
btVector3 n1, n2, n3;
getNormals(index, &n1, &n2, &n3);
// Compute the Barycentric coordinates of position inside triangle
// p1, p2, p3.
btVector3 edge1 = p2 - p1;
btVector3 edge2 = p3 - p1;
// Area of triangle ABC
btScalar p1p2p3 = edge1.cross(edge2).length2();
// Area of BCP
btScalar p2p3p = (p3 - p2).cross(position - p2).length2();
// Area of CAP
btScalar p3p1p = edge2.cross(position - p3).length2();
btScalar s = btSqrt(p2p3p / p1p2p3);
btScalar t = btSqrt(p3p1p / p1p2p3);
btScalar w = 1.0f - s - t;
#ifdef NORMAL_DEBUGGING
btVector3 regen_position = s * p1 + t * p2 + w * p3;
if((regen_position - position).length2() >= 0.0001f)
{
printf("bary:\n");
printf("new: %f %f %f\n", regen_position.getX(),regen_position.getY(),regen_position.getZ());
printf("old: %f %f %f\n", position.getX(), position.getY(),position.getZ());
printf("stw: %f %f %f\n", s, t, w);
printf("p1: %f %f %f\n", p1.getX(),p1.getY(),p1.getZ());
printf("p2: %f %f %f\n", p2.getX(),p2.getY(),p2.getZ());
printf("p3: %f %f %f\n", p3.getX(),p3.getY(),p3.getZ());
printf("pos: %f %f %f\n", position.getX(),position.getY(),position.getZ());
}
#endif
return s*n1 + t*n2 + w*n3;
} // getInterpolatedNormal
/*
* hitFruit
* aktualisiert comboCount und Punkte und spielt sounds ab
* @param pos Position der Frucht
*/
void GameModeTraining::hitFruit(btVector3 pos) {
playSound(hit_fruit, pos.getX(), pos.getZ());
if(gamestate->getStatus() == ACTIVE) {
this->comboTime = 1.0;
++(this->comboCount);
this->gamestate->changePoints(1);
}
}
bool
CGrassSticks::buildGrassSticks(Ogre::SceneManager* sceneMgr, btDynamicsWorld* dynamicsWorld, btSoftBodyWorldInfo &softBodyWorldInfo)
{
// create our grass mesh, and create a grass entity from it
if (!sceneMgr->hasEntity(GRASS_MESH_NAME))
{
createGrassMesh();
} // End if
const int n=16;
const int sg=4;
const btScalar sz=16;
const btScalar hg=4;
const btScalar in=1/(btScalar)(n-1);
int index = 0;
for(int y=0;y<n;++y)
{
for(int x=0;x<n;++x)
{
const btVector3 org(-sz+sz*2*x*in,
1,
-sz+sz*2*y*in);
btSoftBody* psb=btSoftBodyHelpers::CreateRope(softBodyWorldInfo, org,
org+btVector3(hg*0.001f,hg,0),
sg,
1);
psb->m_cfg.kDP = 0.005f;
psb->m_cfg.kCHR = 0.1f;
for(int i=0;i<3;++i)
{
psb->generateBendingConstraints(2+i);
}
psb->setMass(1,0);
psb->setTotalMass(0.01f);
static_cast<btSoftRigidDynamicsWorld*>(dynamicsWorld)->addSoftBody(psb);
const Ogre::String& strIndex = Ogre::StringConverter::toString(index++);
Ogre::Entity* grass = sceneMgr->createEntity("Grass" + strIndex, GRASS_MESH_NAME);
Ogre::SceneNode* node = sceneMgr->getRootSceneNode()->createChildSceneNode("node_grass_" + strIndex
,Ogre::Vector3(org.getX(), org.getY(), org.getZ())
,Ogre::Quaternion(Ogre::Degree(0), Vector3::UNIT_Y));
node->attachObject(grass);
node->scale(1.0f, Ogre::Math::RangeRandom(0.85f, 1.15f), 1.0f);
node->setVisible(true);
psb->setUserPointer((void*)(grass->getSubEntity(0)));
} // End for
} // End for
dynamicsWorld->setInternalTickCallback(&CGrassSticks::simulationTickCallback);
return true;
}
void OgreDebugDrawer::drawTriangle( const btVector3 &v0, const btVector3 &v1, const btVector3 &v2, const btVector3 &color, btScalar alpha )
{
ColourValue c( color.getX(), color.getY(), color.getZ(), alpha );
c.saturate();
mTriangles->position( cvt(v0) );
mTriangles->colour( c );
mTriangles->position( cvt(v1) );
mTriangles->colour( c );
mTriangles->position( cvt(v2) );
mTriangles->colour( c );
}
void btRigidBody::setGravity(const btVector3& acceleration)
{
btAssert(!std::isnan(acceleration.getX()));
btAssert(!std::isnan(acceleration.getY()));
btAssert(!std::isnan(acceleration.getZ()));
if (m_inverseMass != btScalar(0.0))
{
m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
}
m_gravity_acceleration = acceleration;
}
void SingularityDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor) {
btDebugVertex l0, l1;
l0.r = fromColor.getX();
l0.g = fromColor.getY();
l0.b = fromColor.getZ();
l0.x = from.getX();
l0.y = from.getY();
l0.z = from.getZ();
l1.r = toColor.getX();
l1.g = toColor.getY();
l1.b = toColor.getZ();
l1.x = to.getX();
l1.y = to.getY();
l1.z = to.getZ();
m_lineVertexList.push_back(l0);
m_lineVertexList.push_back(l1);
}
btVector3 EntityPhysic::colVelocity(btVector3 velocity_)
{
int MAX=120;
int MIN=50;
if(velocity_.length()>MAX)
{
float radio=velocity_.length()/MAX;
velocity_=btVector3(velocity_.getX()/radio,
velocity_.getY()/radio,
velocity_.getZ()/radio);
}
if(velocity_.length()<MIN)
{
float radio=velocity_.length()/MIN;
velocity_=btVector3(velocity_.getX()/radio,
velocity_.getY()/radio,
velocity_.getZ()/radio);
}
return velocity_;
}
void GLDebugDrawer::drawBox (const btVector3& boxMin, const btVector3& boxMax, const btVector3& color, btScalar alpha) {
btVector3 halfExtent = (boxMax - boxMin) * btScalar(0.5f);
btVector3 center = (boxMax + boxMin) * btScalar(0.5f);
//glEnable(GL_BLEND); // Turn blending On
//glBlendFunc(GL_SRC_ALPHA, GL_ONE);
glColor4f (color.getX(), color.getY(), color.getZ(), alpha);
glPushMatrix ();
glTranslatef (center.getX(), center.getY(), center.getZ());
glScaled(2*halfExtent[0], 2*halfExtent[1], 2*halfExtent[2]);
glPopMatrix ();
//glDisable(GL_BLEND);
}
void GLDebugDrawer::drawContactPoint(const btVector3& pointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color) {
{
btVector3 to=pointOnB+normalOnB*1;//distance;
const btVector3&from = pointOnB;
glColor4f(color.getX(), color.getY(), color.getZ(),1.f);
glBegin(GL_LINES);
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
}
}
void BulletDebugDrawer::drawSphere( btScalar radius, const btTransform& transform, const btVector3& color )
{
ci::ColorA colorA = ci::ColorA( color.getX(), color.getY(), color.getZ() );
ci::gl::enableWireframe();
ci::gl::color( colorA );
ci::gl::pushMatrices();
ci::gl::translate( fromBullet( transform.getOrigin() ) );
ci::gl::rotate( fromBullet( transform.getRotation() ) );
ci::gl::drawSphere( ci::Vec3f::zero(), radius, 20 );
ci::gl::popMatrices();
ci::gl::disableWireframe();
}
void btOptimizedBvh::refitPartial(btStridingMeshInterface* meshInterface,const btVector3& aabbMin,const btVector3& aabbMax)
{
//incrementally initialize quantization values
btAssert(m_useQuantization);
btAssert(aabbMin.getX() > m_bvhAabbMin.getX());
btAssert(aabbMin.getY() > m_bvhAabbMin.getY());
btAssert(aabbMin.getZ() > m_bvhAabbMin.getZ());
btAssert(aabbMax.getX() < m_bvhAabbMax.getX());
btAssert(aabbMax.getY() < m_bvhAabbMax.getY());
btAssert(aabbMax.getZ() < m_bvhAabbMax.getZ());
///we should update all quantization values, using updateBvhNodes(meshInterface);
///but we only update chunks that overlap the given aabb
unsigned short quantizedQueryAabbMin[3];
unsigned short quantizedQueryAabbMax[3];
quantize(&quantizedQueryAabbMin[0],aabbMin,0);
quantize(&quantizedQueryAabbMax[0],aabbMax,1);
int i;
for (i=0;i<this->m_SubtreeHeaders.size();i++)
{
btBvhSubtreeInfo& subtree = m_SubtreeHeaders[i];
//PCK: unsigned instead of bool
unsigned overlap = testQuantizedAabbAgainstQuantizedAabb(quantizedQueryAabbMin,quantizedQueryAabbMax,subtree.m_quantizedAabbMin,subtree.m_quantizedAabbMax);
if (overlap != 0)
{
updateBvhNodes(meshInterface,subtree.m_rootNodeIndex,subtree.m_rootNodeIndex+subtree.m_subtreeSize,i);
subtree.setAabbFromQuantizeNode(m_quantizedContiguousNodes[subtree.m_rootNodeIndex]);
}
}
}
static Ogre::ColourValue
toOgreColour(
const btVector3& colour,
btScalar alpha = 1.0
) {
Ogre::ColourValue ogreColour(
colour.getX(),
colour.getY(),
colour.getZ(),
alpha
);
ogreColour.saturate();
return ogreColour;
}
void btCollisionShape::calculateTemporalAabb(const btTransform& curTrans,const btVector3& linvel,const btVector3& angvel,btScalar timeStep, btVector3& temporalAabbMin,btVector3& temporalAabbMax) const
{
//start with static aabb
getAabb(curTrans,temporalAabbMin,temporalAabbMax);
btScalar temporalAabbMaxx = temporalAabbMax.getX();
btScalar temporalAabbMaxy = temporalAabbMax.getY();
btScalar temporalAabbMaxz = temporalAabbMax.getZ();
btScalar temporalAabbMinx = temporalAabbMin.getX();
btScalar temporalAabbMiny = temporalAabbMin.getY();
btScalar temporalAabbMinz = temporalAabbMin.getZ();
// add linear motion
btVector3 linMotion = linvel*timeStep;
///@todo: simd would have a vector max/min operation, instead of per-element access
if (linMotion.x() > btScalar(0.))
temporalAabbMaxx += linMotion.x();
else
temporalAabbMinx += linMotion.x();
if (linMotion.y() > btScalar(0.))
temporalAabbMaxy += linMotion.y();
else
temporalAabbMiny += linMotion.y();
if (linMotion.z() > btScalar(0.))
temporalAabbMaxz += linMotion.z();
else
temporalAabbMinz += linMotion.z();
//add conservative angular motion
btScalar angularMotion = angvel.length() * getAngularMotionDisc() * timeStep;
btVector3 angularMotion3d(angularMotion,angularMotion,angularMotion);
temporalAabbMin = btVector3(temporalAabbMinx,temporalAabbMiny,temporalAabbMinz);
temporalAabbMax = btVector3(temporalAabbMaxx,temporalAabbMaxy,temporalAabbMaxz);
temporalAabbMin -= angularMotion3d;
temporalAabbMax += angularMotion3d;
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA),
#ifdef _BT_USE_CENTER_LIMIT_
m_limit(),
#endif
m_angularOnly(false), m_enableAngularMotor(false),
m_useSolveConstraintObsolete(HINGE_USE_OBSOLETE_SOLVER),
m_useOffsetForConstraintFrame(HINGE_USE_FRAME_OFFSET),
m_useReferenceFrameA(useReferenceFrameA),
m_flags(0),
m_normalCFM(0),
m_normalERP(0),
m_stopCFM(0),
m_stopERP(0)
{
// since no frame is given, assume this to be zero angle and just pick rb transform axis
// fixed axis in worldspace
btVector3 rbAxisA1, rbAxisA2;
btPlaneSpace1(axisInA, rbAxisA1, rbAxisA2);
m_rbAFrame.getOrigin() = pivotInA;
m_rbAFrame.getBasis().setValue( rbAxisA1.getX(),rbAxisA2.getX(),axisInA.getX(),
rbAxisA1.getY(),rbAxisA2.getY(),axisInA.getY(),
rbAxisA1.getZ(),rbAxisA2.getZ(),axisInA.getZ() );
btVector3 axisInB = rbA.getCenterOfMassTransform().getBasis() * axisInA;
btQuaternion rotationArc = shortestArcQuat(axisInA,axisInB);
btVector3 rbAxisB1 = quatRotate(rotationArc,rbAxisA1);
btVector3 rbAxisB2 = axisInB.cross(rbAxisB1);
m_rbBFrame.getOrigin() = rbA.getCenterOfMassTransform()(pivotInA);
m_rbBFrame.getBasis().setValue( rbAxisB1.getX(),rbAxisB2.getX(),axisInB.getX(),
rbAxisB1.getY(),rbAxisB2.getY(),axisInB.getY(),
rbAxisB1.getZ(),rbAxisB2.getZ(),axisInB.getZ() );
#ifndef _BT_USE_CENTER_LIMIT_
//start with free
m_lowerLimit = btScalar(1.0f);
m_upperLimit = btScalar(-1.0f);
m_biasFactor = 0.3f;
m_relaxationFactor = 1.0f;
m_limitSoftness = 0.9f;
m_solveLimit = false;
#endif
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
void BulletDebugDrawer::drawCylinder( btScalar radius, btScalar halfHeight, int upAxis, const btTransform& transform, const btVector3& color )
{
btIDebugDraw::drawCylinder( radius, halfHeight, upAxis, transform, color );
return;
ci::ColorA colorA = ci::ColorA( color.getX(), color.getY(), color.getZ() );
ci::gl::enableWireframe();
ci::gl::color( colorA );
ci::gl::pushMatrices();
ci::gl::translate( fromBullet( transform.getOrigin() ) );
ci::gl::rotate( fromBullet( transform.getRotation() ) );
ci::gl::drawCylinder( radius, radius, halfHeight, 20, 3 );
ci::gl::popMatrices();
ci::gl::disableWireframe();
}
void DebugDraw::drawLine(const btVector3& from, const btVector3& to, const btVector3& fromColor, const btVector3& toColor) {
//if (debugMode > 0) {
Color3DVertex v1, v2;
v1.x = from.getX();
v1.y = from.getY();
v1.z = from.getZ();
v1.r = fromColor.getX() * 255;
v1.g = fromColor.getY() * 255;
v1.b = fromColor.getZ() * 255;
v1.a = 255;
v2.x = to.getX();
v2.y = to.getY();
v2.z = to.getZ();
v2.r = toColor.getX() * 255;
v2.g = toColor.getY() * 255;
v2.b = toColor.getZ() * 255;
v2.a = 255;
vertices.push_back(v1);
vertices.push_back(v2);
//}
}
CIwFSphere Physics::GetBSphereFromRigidBody( const btRigidBody* b )
{
CIwFVec3 pos;
const btVector3 centerOfMass = b->getCenterOfMassPosition();
pos.x = centerOfMass.getX();
pos.y = centerOfMass.getY();
pos.z = centerOfMass.getZ();
btVector3 vecMaxEdge, vecMinEdge;
b->getAabb( vecMinEdge, vecMaxEdge );
const float r = static_cast<float>( fabs( vecMaxEdge.getZ() - vecMinEdge.getZ() ) );
const CIwFSphere sphere = CIwFSphere( pos, r );
return sphere;
}
btScalar btSphereBoxCollisionAlgorithm::getSpherePenetration( btVector3 const &boxHalfExtent, btVector3 const &sphereRelPos, btVector3 &closestPoint, btVector3& normal )
{
//project the center of the sphere on the closest face of the box
btScalar faceDist = boxHalfExtent.getX() - sphereRelPos.getX();
btScalar minDist = faceDist;
closestPoint.setX( boxHalfExtent.getX() );
normal.setValue(btScalar(1.0f), btScalar(0.0f), btScalar(0.0f));
faceDist = boxHalfExtent.getX() + sphereRelPos.getX();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setX( -boxHalfExtent.getX() );
normal.setValue(btScalar(-1.0f), btScalar(0.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getY() - sphereRelPos.getY();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setY( boxHalfExtent.getY() );
normal.setValue(btScalar(0.0f), btScalar(1.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getY() + sphereRelPos.getY();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setY( -boxHalfExtent.getY() );
normal.setValue(btScalar(0.0f), btScalar(-1.0f), btScalar(0.0f));
}
faceDist = boxHalfExtent.getZ() - sphereRelPos.getZ();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setZ( boxHalfExtent.getZ() );
normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(1.0f));
}
faceDist = boxHalfExtent.getZ() + sphereRelPos.getZ();
if (faceDist < minDist)
{
minDist = faceDist;
closestPoint = sphereRelPos;
closestPoint.setZ( -boxHalfExtent.getZ() );
normal.setValue(btScalar(0.0f), btScalar(0.0f), btScalar(-1.0f));
}
return minDist;
}
void RigidSceneDebug::drawContactPoint(const btVector3& pointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
{
btVector3 to=pointOnB+normalOnB*distance;
const btVector3&from = pointOnB;
glBegin(GL_LINES);
glColor3f(color.getX(), color.getY(), color.getZ());
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
// glRasterPos3f(from.x(), from.y(), from.z());
// char buf[12];
// sprintf(buf," %d",lifeTime);
// BMF_DrawString(BMF_GetFont(BMF_kHelvetica10),buf);
}
int btTriangleMesh::findOrAddVertex(const btVector3& vertex, bool removeDuplicateVertices)
{
//return index of new/existing vertex
///@todo: could use acceleration structure for this
if (m_use4componentVertices)
{
if (removeDuplicateVertices)
{
for (int i=0;i< m_4componentVertices.size();i++)
{
if ((m_4componentVertices[i]-vertex).length2() <= m_weldingThreshold)
{
return i;
}
}
}
m_indexedMeshes[0].m_numVertices++;
m_4componentVertices.push_back(vertex);
m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_4componentVertices[0];
return m_4componentVertices.size()-1;
} else
{
if (removeDuplicateVertices)
{
for (int i=0;i< m_3componentVertices.size();i+=3)
{
btVector3 vtx(m_3componentVertices[i],m_3componentVertices[i+1],m_3componentVertices[i+2]);
if ((vtx-vertex).length2() <= m_weldingThreshold)
{
return i/3;
}
}
}
m_3componentVertices.push_back((float)vertex.getX());
m_3componentVertices.push_back((float)vertex.getY());
m_3componentVertices.push_back((float)vertex.getZ());
m_indexedMeshes[0].m_numVertices++;
m_indexedMeshes[0].m_vertexBase = (unsigned char*)&m_3componentVertices[0];
return (m_3componentVertices.size()/3)-1;
}
}
void GLDebugDrawer::drawContactPoint(
const btVector3 &pointOnB, const btVector3 &normalOnB,
btScalar distance, int lifeTime, const btVector3 &color) {
btVector3 to = pointOnB + normalOnB * 1;//distance;
const btVector3 &from = pointOnB;
glColor4f(color.getX(), color.getY(), color.getZ(),1.f);
//glColor4f(0,0,0,1.f);
Mygl gl(2, 0, false, false, false);
gl.glBegin(GL_LINES);
gl.glVertex(from, to);
gl.glEnd();
// glRasterPos3f(from.x(), from.y(), from.z());
// char buf[12];
// sprintf(buf," %d",lifeTime);
// BMF_DrawString(BMF_GetFont(BMF_kHelvetica10), buf);
}
/**************************************************************************************
| build grass cluster using static geometry
**************************************************************************************/
bool
CGrassSticks::buildStaticGrassSticks(Ogre::SceneManager* sceneMgr)
{
// create our grass mesh, and create a grass entity from it
if (!sceneMgr->hasEntity(GRASS_MESH_NAME))
{
createGrassMesh();
} // End if
mField = sceneMgr->createStaticGeometry("GrassField");
mField->setRegionDimensions(Ogre::Vector3(1000, 1000, 1000));
mField->setOrigin(Ogre::Vector3(0,0,0));
Ogre::Entity* ent = sceneMgr->createEntity("Grass", GRASS_MESH_NAME);
const int n=16;
const int sg=4;
const btScalar sz=16;
const btScalar hg=4;
const btScalar in=1/(btScalar)(n-1);
for(int y=0;y<n;++y)
{
for(int x=0;x<n;++x)
{
const btVector3 org(-sz+sz*2*x*in,
1,
-sz+sz*2*y*in);
Ogre::Vector3 pos(org.getX(), org.getY(), org.getZ());
Ogre::Quaternion ori(Ogre::Degree(0), Vector3::UNIT_Y);
Ogre::Vector3 scale(1, Ogre::Math::RangeRandom(0.85f, 1.15f), 1);
mField->addEntity(ent, pos, ori, scale);
} // End for
} // End for
mField->build();
return true;
}