const btVector3 rotate( const btQuaternion& quat, const btVector3 & vec )
{
float tmpX, tmpY, tmpZ, tmpW;
tmpX = ( ( ( quat.getW() * vec.getX() ) + ( quat.getY() * vec.getZ() ) ) - ( quat.getZ() * vec.getY() ) );
tmpY = ( ( ( quat.getW() * vec.getY() ) + ( quat.getZ() * vec.getX() ) ) - ( quat.getX() * vec.getZ() ) );
tmpZ = ( ( ( quat.getW() * vec.getZ() ) + ( quat.getX() * vec.getY() ) ) - ( quat.getY() * vec.getX() ) );
tmpW = ( ( ( quat.getX() * vec.getX() ) + ( quat.getY() * vec.getY() ) ) + ( quat.getZ() * vec.getZ() ) );
return btVector3(
( ( ( ( tmpW * quat.getX() ) + ( tmpX * quat.getW() ) ) - ( tmpY * quat.getZ() ) ) + ( tmpZ * quat.getY() ) ),
( ( ( ( tmpW * quat.getY() ) + ( tmpY * quat.getW() ) ) - ( tmpZ * quat.getX() ) ) + ( tmpX * quat.getZ() ) ),
( ( ( ( tmpW * quat.getZ() ) + ( tmpZ * quat.getW() ) ) - ( tmpX * quat.getY() ) ) + ( tmpY * quat.getX() ) )
);
}
void PhysicsDebugDraw::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor)
{
debugRenderer->drawLine(
Vec3(from.getX(), from.getY(), from.getZ()),
Vec3(to.getX(), to.getY(), to.getZ()),
Vec4(fromColor.getX(), fromColor.getY(), fromColor.getZ(), 1.f),
Vec4(toColor.getX(), toColor.getY(), toColor.getZ(), 1.f)
);
}
IMesh* MeshTools::createMeshFromSoftBody(btSoftBody* softBody)
{
SMeshBuffer* buffer = new SMeshBuffer();
video::S3DVertex vtx;
vtx.Color.set(255,255,255,255);
/* Each soft body contain an array of vertices (nodes/particles_mass) */
btSoftBody::tNodeArray& nodes(softBody->m_nodes);
// Convert bullet nodes to vertices
for(int i=0;i<nodes.size();++i)
{
const btSoftBody::Node& n=nodes[i];
const btVector3 normal=n.m_n;
const btVector3 pos=n.m_x;
vtx.Pos.set(pos.getX(), pos.getY(), pos.getZ());
vtx.Normal.set(normal.getX(), normal.getY(), normal.getZ());
// TODO: calculate texture coords
//vtx.TCoords.set(tsx, tsy);
buffer->Vertices.push_back(vtx);
}
// Convert triangles of the softbody to an index list
for(int i=0;i<softBody->m_faces.size();++i)
{
auto faces = softBody->m_faces;
btSoftBody::Node* node_0=faces[i].m_n[0];
btSoftBody::Node* node_1=faces[i].m_n[1];
btSoftBody::Node* node_2=faces[i].m_n[2];
const int indices[] =
{
int(node_0-&nodes[0]),
int(node_1-&nodes[0]),
int(node_2-&nodes[0])
};
for(int j=0;j<3;++j)
buffer->Indices.push_back(indices[j]);
}
buffer->recalculateBoundingBox();
// Default the mesh to stream because most likely we will be updating
// the vertex positions every frame to deal with softbody movement.
buffer->setHardwareMappingHint(EHM_STREAM);
SMesh* mesh = new SMesh();
mesh->addMeshBuffer(buffer);
mesh->recalculateBoundingBox();
buffer->drop();
return mesh;
}
void PhysicsDebugDraw::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha)
{
debugRenderer->drawTriangle(
Vec3(a.getX(), a.getY(), a.getZ()),
Vec3(b.getX(), b.getY(), b.getZ()),
Vec3(c.getX(), c.getY(), c.getZ()),
Vec4(color.getX(), color.getY(), color.getZ(), alpha)
);
}
void DebugDrawer::drawLine(const btVector3 &from, const btVector3 &to,
const btVector3 &color) {
glBegin(GL_LINES);
glColor3f(color.getX(), color.getY(), color.getZ());
glVertex3f(from.getX(), from.getY(), from.getZ());
glVertex3f(to.getX(), to.getY(), to.getZ());
glEnd();
}
/// conservative test for overlap between two aabbs
static bool TestAabbAgainstAabb2(const btVector3 &aabbMin1, const btVector3 &aabbMax1,
const btVector3 &aabbMin2, const btVector3 &aabbMax2)
{
bool overlap = true;
overlap = (aabbMin1.getX() > aabbMax2.getX() || aabbMax1.getX() < aabbMin2.getX()) ? false : overlap;
overlap = (aabbMin1.getZ() > aabbMax2.getZ() || aabbMax1.getZ() < aabbMin2.getZ()) ? false : overlap;
overlap = (aabbMin1.getY() > aabbMax2.getY() || aabbMax1.getY() < aabbMin2.getY()) ? false : overlap;
return overlap;
}
void NEGLDebugDrawer::drawLine(const btVector3& from, const btVector3& to, const btVector3& fromColor) {
glBegin(GL_LINES);
glColor3f(1.0f, 0.04f, 0.0f);
glVertex3d(from.getX(), from.getY(), from.getZ());
glColor3f(1.0f, 0.04f, 0.0f);
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
}
void BulletGLDebugDraw::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor)
{
glBegin(GL_LINES);
glColor3f(fromColor.getX(), fromColor.getY(), fromColor.getZ());
glVertex3d(from.getX(), from.getY(), from.getZ());
glColor3f(toColor.getX(), toColor.getY(), toColor.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
}
void BulletDebugger::drawLine(const btVector3 & from, const btVector3 & to, const btVector3 & fromColor, const btVector3 & toColor) {
points->push_back(from.getX());
points->push_back(from.getY());
points->push_back(from.getZ());
points->push_back(1.0f);
points->push_back(to.getX());
points->push_back(to.getY());
points->push_back(to.getZ());
points->push_back(1.0f);
}
void proPhysDebug::drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
peUint r = (peUint)color.getX()*255;
peUint g = (peUint)color.getY()*255;
peUint b = (peUint)color.getZ()*255;
peUint uColor = (b<<16) + (g<<8) + (r) + (0xFF<<24);
proVec3f lineFrom = Vec3f(from.getX(), from.getY(), from.getZ());
proVec3f lineTo = Vec3f(to.getX(), to.getY(), to.getZ());
proRenderDebug::Instance()->DrawLine3D(lineFrom, lineTo, uColor);
}
void GLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& fromColor, const btVector3& toColor)
{
//std::cout << "drawline" << std::endl;
glBegin(GL_LINES);
glColor3f(fromColor.getX(), fromColor.getY(), fromColor.getZ());
glVertex3d(from.getX(), from.getY(), from.getZ());
glColor3f(toColor.getX(), toColor.getY(), toColor.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
}
void nau::world::BulletDebugger::drawLine(const btVector3 & from, const btVector3 & to, const btVector3 & color) {
points->push_back(from.getX());
points->push_back(from.getY());
points->push_back(from.getZ());
points->push_back(1.0f);
points->push_back(to.getX());
points->push_back(to.getY());
points->push_back(to.getZ());
points->push_back(1.0f);
}
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),
#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_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() );
#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 BulletDebugDraw::drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
// DEBUG_PRINTF( "%s\n", __PRETTY_FUNCTION__ );
Integration::DynamicsDebugVertex vertex;
vertex.position = Vector3( from.getX(), from.getY(), from.getZ() );
vertex.color = Vector4( color.getX(), color.getY(), color.getZ(), 1.0f );
mVertices.push_back( vertex );
vertex.position = Vector3( to.getX(), to.getY(), to.getZ());
mVertices.push_back( vertex );
}
void DebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
if(!m_line_callback)
return;
float data[9] = {
from.getX(), from.getY(), from.getZ(),
to.getX(), to.getY(), to.getZ(),
color.getX(), color.getY(), color.getZ()
};
m_line_callback(&data[0]);
}
void RigidSceneDebug::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha)
{
// const btVector3 n=cross(b-a,c-a).normalized();
glBegin(GL_TRIANGLES);
glColor4f(color.getX(), color.getY(), color.getZ(),alpha);
// glNormal3d(n.getX(),n.getY(),n.getZ());
glVertex3d(a.getX(),a.getY(),a.getZ());
glVertex3d(b.getX(),b.getY(),b.getZ());
glVertex3d(c.getX(),c.getY(),c.getZ());
glEnd();
}
void CGmObjPhysMan::CGLDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
if (m_debugMode > 0)
{
glBegin(GL_LINES);
glColor4f(color.getX(), color.getY(), color.getZ(),1.f);
glVertex3d(from.getX(), from.getY(), from.getZ());
glVertex3d(to.getX(), to.getY(), to.getZ());
glEnd();
}
}
double Leaf::bulletScalar(const btVector3& vec1, const btVector3& vec2)
{
double X1 = vec1.getX();
double X2 = vec2.getX();
double Y1 = vec1.getY();
double Y2 = vec2.getY();
double Z1 = vec1.getZ();
double Z2 = vec2.getZ();
double ans = (X1*X2) + (Y1*Y2) + (Z1*Z2);
ans = abs(ans);
return ans;
}
void GLDebugDrawer::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha) {
{
const btVector3 n=btCross(b-a,c-a).normalized();
glBegin(GL_TRIANGLES);
glColor4f(color.getX(), color.getY(), color.getZ(),alpha);
glNormal3d(n.getX(),n.getY(),n.getZ());
glVertex3d(a.getX(),a.getY(),a.getZ());
glVertex3d(b.getX(),b.getY(),b.getZ());
glVertex3d(c.getX(),c.getY(),c.getZ());
glEnd();
}
}
void CGmObjPhysMan::CGLDebugDrawer::drawTriangle(const btVector3& a,const btVector3& b,const btVector3& c,const btVector3& color,btScalar alpha)
{
if (m_debugMode > 0)
{
const btVector3 n=cross(b-a,c-a).normalized();
glBegin(GL_TRIANGLES);
glColor4f(color.getX(), color.getY(), color.getZ(),alpha);
glNormal3d(n.getX(),n.getY(),n.getZ());
glVertex3d(a.getX(),a.getY(),a.getZ());
glVertex3d(b.getX(),b.getY(),b.getZ());
glVertex3d(c.getX(),c.getY(),c.getZ());
glEnd();
}
}
btHingeConstraint::btHingeConstraint(btRigidBody& rbA,const btVector3& pivotInA,const btVector3& axisInA, bool useReferenceFrameA)
:btTypedConstraint(HINGE_CONSTRAINT_TYPE, rbA), 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()
{
// 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() );
m_referenceSign = m_useReferenceFrameA ? btScalar(-1.f) : btScalar(1.f);
}
void physGrapplePoint(){
lookAt = GetLookAt();
glm::vec3 at = glm::vec3(lookAt.x,lookAt.y,lookAt.z);
glm::vec3 targ = glm::vec3(tmp.getX(),tmp.getY(),tmp.getZ());
glm::vec3 loc = targ-at;
float dist = sqrt(loc.x*loc.x+loc.y*loc.y+loc.z*loc.z);
targ-=at;
targ*=10;
//
dist/=3;
dist = dist>1?dist:1;
dist = targ.y>0?dist:dist/1.4;
//
player->setLinearVelocity(btVector3(targ.x,targ.y/dist,targ.z));
if(dist<1.1 && !hold){
if(flip==0){
flip = 1;
//printf("pls rotate camera to normal of building\n");
// rotateCamera(pi);
}
playerJump = 1;
playerGrappleActive=0;
//setPlayerSpeed(0,10,0);
//printf("reset\n");
}
}
void shootBox(const btVector3& destination)
{
float mass = 1.f;
btTransform startTransform;
startTransform.setIdentity();
btVector3 camPos = getCameraPosition();
startTransform.setOrigin(camPos);
const btScalar BOX_DIMENSIONS = 3.0f;
btBoxShape* box = new btBoxShape( btVector3(BOX_DIMENSIONS, 0.1f, BOX_DIMENSIONS*4/3) );
box->initializePolyhedralFeatures();
m_shootBoxShape = box;
btRigidBody* body = localCreateRigidBody(mass, startTransform,m_shootBoxShape);
body->setLinearFactor(btVector3(1,1,1));
//body->setRestitution(1);
btVector3 linVel(destination[0]-camPos[0],destination[1]-camPos[1],destination[2]-camPos[2]);
linVel.normalize();
linVel*=m_ShootBoxInitialSpeed;
body->getWorldTransform().setOrigin(camPos);
body->getWorldTransform().setRotation(btQuaternion(0,0,0,1));
body->setLinearVelocity(linVel);
body->setAngularVelocity(btVector3(0,0,0));
body->setCcdMotionThreshold(0.5);
body->setCcdSweptSphereRadius(0.4f);//value should be smaller (embedded) than the half extends of the box (see ::setShootBoxShape)
LOGI("shootBox uid=%d\n", body->getBroadphaseHandle()->getUid());
LOGI("camPos=%f,%f,%f\n",camPos.getX(),camPos.getY(),camPos.getZ());
LOGI("destination=%f,%f,%f\n",destination.getX(),destination.getY(),destination.getZ());
}
/*
* hitBomb
* aktualisiert comboCount und Punkte und spielt sounds ab
* @param pos Position der Bombe
*/
void GameModeTraining::hitBomb(btVector3 pos) {
playSound(hit_bomb, pos.getX(), pos.getZ());
this->comboTime = 0.0;
this->comboCount = 0;
}
void SingularityDebugDrawer::drawSphere(const btVector3& p, btScalar radius, const btVector3& color) {
glTranslatef(p.getX(), p.getY(), p.getZ());
int lats = 5;
int longs = 5;
int i, j;
for(i = 0; i <= lats; i++) {
btScalar lat0 = SIMD_PI * (-btScalar(0.5) + (btScalar)(i - 1) / lats);
btScalar z0 = radius*sin(lat0);
btScalar zr0 = radius*cos(lat0);
btScalar lat1 = SIMD_PI * (-btScalar(0.5) + (btScalar) i / lats);
btScalar z1 = radius*sin(lat1);
btScalar zr1 = radius*cos(lat1);
glBegin(GL_QUAD_STRIP);
for(j = 0; j <= longs; j++) {
btScalar lng = 2 * SIMD_PI * (btScalar)(j - 1) / longs;
btScalar x = cos(lng);
btScalar y = sin(lng);
glNormal3f(x * zr0, y * zr0, z0);
glVertex3f(x * zr0, y * zr0, z0);
glNormal3f(x * zr1, y * zr1, z1);
glVertex3f(x * zr1, y * zr1, z1);
}
glEnd();
}
glPopMatrix();
}
Mouth::Mouth(btDynamicsWorld* ownerWorld, void* owner, const btVector3& dimensions, float weight, btTransform& offset, btTransform& transform)
{
m_ownerWorld = ownerWorld;
// Add a box body
shape = new btBoxShape( dimensions );
btVector3 localInertia(0,0,0);
if (weight != 0.f) // weight of non zero = dynamic
shape->calculateLocalInertia(weight,localInertia);
myMotionState = new btDefaultMotionState(offset*transform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(weight,myMotionState,shape,localInertia);
body = new btRigidBody(rbInfo);
body->setUserPointer(owner);
body->setDamping(0.05, 0.85);
body->setDeactivationTime(0.001);
body->setSleepingThresholds(1.6, 2.5);
m_ownerWorld->addRigidBody(body);
// Mouth piece
ghostObject = new btPairCachingGhostObject();
ghostObject->setCollisionShape( new btBoxShape( btVector3( dimensions.getX()+0.01f, dimensions.getY()+0.01f, dimensions.getZ()+0.01f ) ) );
ghostObject->setCollisionFlags( btCollisionObject::CF_KINEMATIC_OBJECT | btCollisionObject::CF_NO_CONTACT_RESPONSE );
ghostObject->setWorldTransform(offset*transform);
ghostObject->setUserPointer(owner);
m_ownerWorld->addCollisionObject(ghostObject);
// create a pointer to the body's motionstate
}
void btVector3_to_Vector3(JNIEnv * const &jenv, jobject &target, const btVector3 &source)
{
vector3_ensurefields(jenv, target);
jenv->SetFloatField(target, vector3_x, source.getX());
jenv->SetFloatField(target, vector3_y, source.getY());
jenv->SetFloatField(target, vector3_z, source.getZ());
}
/*
* fruitHitFloor
* spielt sounds ab, aktualisiert die Höchstentfernung wenn höher als letzte
* @param pos Position der Frucht
*/
void GameModeTennis::fruitHitFloor(btVector3 pos) {
int distance = (int)round(sqrt(pow(pos.getX(), 2) + pow(pos.getZ(), 2)));
if (gamestate->getPoints() < distance){
gamestate->setPoints(distance);
}
}
void OgreBtDebugDrawer::drawContactPoint(const btVector3 &PointOnB,
const btVector3& normalOnB, const btScalar distance, const int lifeTime,
const btVector3& color) {
drawContactPoint(OgreBulletUtils::convert(PointOnB),
OgreBulletUtils::convert(normalOnB), distance, lifeTime,
Ogre::ColourValue(color.getX(), color.getY(), color.getZ()));
}
void OgreBtDebugDrawer::drawTriangle(const btVector3 &v0, const btVector3 &v1,
const btVector3 &v2, const btVector3 &color, const btScalar alpha) {
drawTriangle(OgreBulletUtils::convert(v0), OgreBulletUtils::convert(v1),
OgreBulletUtils::convert(v2),
Ogre::ColourValue(color.getX(), color.getY(), color.getZ()),
Ogre::Real(alpha));
}
