void DebugDraw::drawLine(const btVector3& from, const btVector3& to, const btVector3& from_color, const btVector3& to_color) {
sf::Vertex line[] = {
sf::Vertex(sf::Vector2f(from.x(), from.y()), btToSfColor(from_color)),
sf::Vertex(sf::Vector2f(to.x(), to.y()), btToSfColor(to_color))
};
m_target->draw(line, 2, sf::Lines);
}
virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
ScnDebugRenderComponent::pImpl()->drawLine(
MaVec3d( from.x(), from.y(), from.z() ),
MaVec3d( to.x(), to.y(), to.z() ),
RsColour( color.x(), color.y(), color.z(), 1.0f ) );
}
void BeGraphicsModel::draw( unsigned int current_material, const btTransform& transform, const btVector3& scale )
{
// btScalar m[16];
transform.getOpenGLMatrix(m_matrix);
// m_system->matrixPush(GL_MODELVIEW);
glPushMatrix();
// m_system->matrixMult(GL_MODELVIEW, m_matrix);
m_system->matrixMult(m_matrix);
// m_system->matrixLoad(GL_MODELVIEW, m_matrix);
// m_system->matrixPush(GL_MODELVIEW);
// glPushMatrix();
if ( scale.x() != 1.0f || scale.y() != 1.0f || scale.z() != 1.0f )
m_system->matrixScale(GL_MODELVIEW, scale.x(), scale.y(), scale.z());
draw(current_material);
// glPopMatrix();
// m_system->matrixPop(GL_MODELVIEW);
// m_system->matrixPop(GL_MODELVIEW);
glPopMatrix();
//glEnable(GL_CULL_FACE);
//glCullFace(GL_BACK);
}
virtual btScalar addSingleResult(btCollisionWorld::LocalRayResult& rayResult,bool normalInWorldSpace)
{
elfCollision* collision;
m_collisionObject = rayResult.m_collisionObject;
if (normalInWorldSpace)
{
m_hitNormalWorld = rayResult.m_hitNormalLocal;
}
else
{
m_hitNormalWorld = m_collisionObject->getWorldTransform().getBasis()*rayResult.m_hitNormalLocal;
}
m_hitPointWorld.setInterpolate3(m_rayFromWorld, m_rayToWorld, rayResult.m_hitFraction);
collision = elfCreateCollision();
collision->position.x = m_hitPointWorld.x();
collision->position.y = m_hitPointWorld.y();
collision->position.z = m_hitPointWorld.z();
collision->normal.x = m_hitNormalWorld.x();
collision->normal.y = m_hitNormalWorld.y();
collision->normal.z = m_hitNormalWorld.z();
collision->actor = ((elfPhysicsObject*)((btRigidBody*)m_collisionObject)->getUserPointer())->actor;
elfIncRef((elfObject*)collision->actor);
elfAppendListObject(m_list, (elfObject*)collision);
return rayResult.m_hitFraction;
}
void gkPhysicsDebug::drawLine(const btVector3& from, const btVector3& to, const btVector3& color)
{
gkDebugger* dbg = m_physics->getScene()->getDebugger();
if (dbg)
dbg->drawLine(gkVector3(from.x(), from.y(), from.z()), gkVector3(to.x(), to.y(), to.z()), gkVector3(color.x(), color.y(), color.z()));
}
static inline btVector3 getPrincipalInertia(const btVector3 & p, const btScalar & m)
{
return m * btVector3(
p.y() * p.y() + p.z() * p.z(),
p.x() * p.x() + p.z() * p.z(),
p.x() * p.x() + p.y() * p.y());
}
void renderSPH()
{
static const btVector3 activeColor = btVector3(1, 1, 1);
static const btVector3 inactiveColor = btVector3(.3, .3, .7);
for(int n = 0; n < fluid->numParticles(); ++n)
{
btVector3 p = fluid->getPosition(n);
btVector3 vel = fluid->getVelocity(n);
float speed = std::min(1.0f, vel.length() / 1.0f);
btVector3 color = (1.0f - speed) * inactiveColor + speed * activeColor;
glPushMatrix();
glColor3f(color.x(), color.y(), color.z() );
float xt = (p.getX() - minBound.x()) / (maxBound.x() - minBound.x());
float yt = (p.getY() - minBound.y()) / (maxBound.y() - minBound.y());
float zt = (p.getZ() - minBound.z()) / (maxBound.z() - minBound.z());
glTranslatef(xt*scale.x() + origin.x(), yt*scale.y() + origin.y(), zt*scale.z() + origin.z());
glBegin( GL_LINE_LOOP );
GLUquadricObj *quadric;
quadric = gluNewQuadric();
gluQuadricDrawStyle(quadric, GLU_FILL );
gluSphere( quadric , 1.0f , 8 , 8);
glGetError();
gluDeleteQuadric(quadric);
glEnd();
glPopMatrix();
}
}
void PhysicsDebug::drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
D3D11_MAPPED_SUBRESOURCE mapStruct;
ZeroMemory(&mapStruct, sizeof(mapStruct));
ColourVertex vertexSet[] =
{
ColourVertex(D3DXVECTOR3(to.x(), to.y(), to.z()), D3DXVECTOR4(color.x(), color.y(), color.z(), 1.0f)),
ColourVertex(D3DXVECTOR3(from.x(), from.y(), from.z()), D3DXVECTOR4(color.x(), color.y(), color.z(), 1.0f))
};
if(mContext->Map(mBuffer, 0, D3D11_MAP_WRITE_DISCARD, 0, &mapStruct) != S_OK)
{
std::cerr << "Buffer Mapping Failed!" << std::endl;
return;
}
memcpy(mapStruct.pData, &vertexSet[0], sizeof(ColourVertex) * mTotalIndices);
mContext->Unmap(mBuffer, 0);
mContext->IASetPrimitiveTopology(D3D11_PRIMITIVE_TOPOLOGY_LINELIST);
mShader.RenderVertexBuffer(mTotalIndices, mBuffer, mDevice, mContext, &ShaderData(mGameCam));
}
void DebugDraw::drawAabb(const btVector3 &from, const btVector3 &to, const btVector3 &color) {
sf::RectangleShape rect(sf::Vector2f(to.x() - from.x(), to.y() - from.y()));
rect.setPosition(from.x(), from.y());
rect.setFillColor(sf::Color::Transparent);
rect.setOutlineColor(btToSfColor(color));
rect.setOutlineThickness(0.01f);
m_target->draw(rect);
}
void drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color) override {
DebugRenderQueue::addLine(
Vec3(from.x(), from.y(), from.z()),
Vec3(to.x(), to.y(), to.z()),
Vec4(color.x(), color.y(), color.z(), 1.0f)
);
}
void DebugDraw::drawTriangle(const btVector3 &v0, const btVector3 &v1, const btVector3 &v2, const btVector3 &color, btScalar) {
sf::Vertex triangle[] = {
sf::Vertex(sf::Vector2f(v0.x(), v0.y()), btToSfColor(color)),
sf::Vertex(sf::Vector2f(v1.x(), v1.y()), btToSfColor(color)),
sf::Vertex(sf::Vector2f(v2.x(), v2.y()), btToSfColor(color))
};
m_target->draw(triangle, 3, sf::Triangles);
}
void PhysicsDebugRenderer::drawContactPoint(const btVector3 &PointOnB, const btVector3 &normalOnB, btScalar distance, int lifeTime, const btVector3 &color)
{
const glm::vec3 center(PointOnB.x(), PointOnB.y(), PointOnB.z());
const glm::vec3 cubeSize(0.1f, 0.1f, 0.1f);
const glm::vec3 cubeColor(color.x(), color.y(), color.z());
DebugRenderer::DrawCube(center, cubeSize, cubeColor);
}
void PhysicsDebugRenderer::drawLine(const btVector3 &from, const btVector3 &to, const btVector3 &color)
{
const glm::vec3 start(from.x(), from.y(), from.z());
const glm::vec3 end(to.x(), to.y(), to.z());
const glm::vec3 lineColor(color.x(), color.y(), color.z());
DebugRenderer::DrawLine(start, end, lineColor);
}
void glDebugDraw::drawLine(const btVector3 &from, const btVector3 &to,
const btVector3 &color)
{
glColor3f(color.x(), color.y(), color.z());
glBegin(GL_LINES);
glVertex3f(from.x(), from.y(), from.z());
glVertex3f(to.x(), to.y(), to.z());
glEnd();
}
void PhysicsDebugDrawer::drawLine(const btVector3& from, const btVector3& to, const btVector3& color)
{
// Draw a line with the 'from' position being slightly darker.
glm::vec3 glmColor = glm::vec3(color.x(), color.y(), color.z());
lines.positions.push_back(glm::vec3(from.x(), from.y(), from.z()));
lines.colors.push_back(glmColor * 0.80f);
lines.positions.push_back(glm::vec3(to.x(), to.y(), to.z()));
lines.colors.push_back(glmColor);
}
btVector3 btRigidBody::getLocalInertia() const
{
btVector3 inertiaLocal;
const btVector3 inertia = m_invInertiaLocal;
inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
return inertiaLocal;
}
void PhysicsDebugDraw::drawTriangle( const btVector3& a, const btVector3& b, const btVector3& c,const btVector3& color, btScalar alpha )
{
const Vector3 v1 = { a.x(), a.y(), a.z() };
const Vector3 v2 = { b.x(), b.y(), b.z() };
const Vector3 v3 = { c.x(), c.y(), c.z() };
const Color color2 = color::fromFloatRGBA( color.x(), color.y(), color.z(), 1.0f );
const Vector3 points[] = { v1, v2, v3, v1 };
debugrenderer::drawLines( points, TIKI_COUNT( points ), color2 );
}
void PhysicsDebug::drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
mIC->BeginVertex();
mIC->SetPosition3(from.get128());
mIC->SetColor(FromRGBf(color.x(), color.y(), color.z()));
mIC->EndVertex();
mIC->BeginVertex();
mIC->SetPosition3(to.get128());
mIC->SetColor(FromRGBf(color.x(), color.y(), color.z()));
mIC->EndVertex();
}
void updateCollisionSphere(float x, float y, float z, float vx, float vy, float vz)
{
x = (x - origin.x()) / scale.x();
y = (y - origin.y()) / scale.y();
z = (z - origin.z()) / scale.z();
x = x * (maxBound.x() - minBound.x()) + minBound.x();
y = y * (maxBound.y() - minBound.y()) + minBound.y();
z = z * (maxBound.z() - minBound.z()) + minBound.z();
collisionSpherePos = btVector3(x, y, z);
collisionSphereVel = btVector3(vx, vy, vz);
}
virtual void drawSphere (const btVector3& p, btScalar radius, const btVector3& color)
{
double p_[3] = {p.x() / bulletWorldScalingFactor,
p.y() / bulletWorldScalingFactor,
p.z() / bulletWorldScalingFactor};
double color_[3] = {color.x(), color.y(), color.z()};
if(callbacks_.drawSphere)
(*callbacks_.drawSphere)(p_, radius / bulletWorldScalingFactor, color_, arg_);
else
btIDebugDraw::drawSphere(p/bulletWorldScalingFactor, radius / bulletWorldScalingFactor, color);
}
void PhysicsDebugDrawer::drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
data.push_back( from.x());
data.push_back( from.y());
data.push_back( from.z());
data.push_back( to.x());
data.push_back( to.y());
data.push_back( to.z());
}
void btWorldFactory::createBoxAreaSpawn(QVariantList &spawnsList, btVector3 size, btVector3 pos)
{
QVariantMap spawnMap;
spawnMap.insert("type", "boxArea");
spawnMap.insert("sizeX", (double)size.x());
spawnMap.insert("sizeY", (double)size.y());
spawnMap.insert("sizeZ", (double)size.z());
spawnMap.insert("posX", (double)pos.x());
spawnMap.insert("posY", (double)pos.y());
spawnMap.insert("posZ", (double)pos.z());
spawnsList.append(spawnMap);
}
void PhysicsDebugDraw::drawLine( const btVector3& from, const btVector3& to, const btVector3& color )
{
const Color color2 = color::fromFloatRGBA( color.x(), color.y(), color.z(), 1.0f );
const Vector3 points[] =
{
{ from.x(), from.y(), from.z() },
{ to.x(), to.y(), to.z() }
};
debugrenderer::drawLines( points, TIKI_COUNT( points ), color2 );
}
virtual void drawContactPoint(const btVector3& PointOnB,const btVector3& normalOnB,btScalar distance,int lifeTime,const btVector3& color)
{
double PointOnB_[3] = {PointOnB.x() / bulletWorldScalingFactor,
PointOnB.y() / bulletWorldScalingFactor,
PointOnB.z() / bulletWorldScalingFactor};
double normalOnB_[3] = {normalOnB.x() / bulletWorldScalingFactor,
normalOnB.y() / bulletWorldScalingFactor,
normalOnB.z() / bulletWorldScalingFactor};
double color_[3] = {color.x(), color.y(), color.z()};
if(callbacks_.drawContactPoint)
(*callbacks_.drawContactPoint)(PointOnB_, normalOnB_, distance, lifeTime, color_, arg_);
}
virtual void drawLine(const btVector3& from,const btVector3& to,const btVector3& color)
{
double from_[3] = {from.x() / bulletWorldScalingFactor,
from.y() / bulletWorldScalingFactor,
from.z() / bulletWorldScalingFactor};
double to_[3] = {to.x() / bulletWorldScalingFactor,
to.y() / bulletWorldScalingFactor,
to.z() / bulletWorldScalingFactor};
double color_[3] = {color.x(), color.y(), color.z()};
if(callbacks_.drawLine)
(*callbacks_.drawLine)(from_, to_, color_, arg_);
}
void InverseTransformPoint3x3(btVector3& out, const btVector3& in, const btTransform& tr)
{
const btMatrix3x3& rot = tr.getBasis();
const btVector3& r0 = rot[0];
const btVector3& r1 = rot[1];
const btVector3& r2 = rot[2];
const btScalar x = r0.x()*in.x() + r1.x()*in.y() + r2.x()*in.z();
const btScalar y = r0.y()*in.x() + r1.y()*in.y() + r2.y()*in.z();
const btScalar z = r0.z()*in.x() + r1.z()*in.y() + r2.z()*in.z();
out.setValue(x, y, z);
}
// void BeGraphicsModel::draw( const float* transform, const float x, const float y, const float z )
void BeGraphicsModel::drawToDepth(const float* transform, const btVector3& scale )
{
m_system->matrixPush(GL_MODELVIEW);
// std::cout << "scale" << std::endl;
m_system->matrixMult(GL_MODELVIEW, transform);
if ( scale.x() != 1.0f || scale.y() != 1.0f || scale.z() != 1.0f )
m_system->matrixScale(GL_MODELVIEW, scale.x(), scale.y(), scale.z());
drawToDepth();
m_system->matrixPop(GL_MODELVIEW);
//glEnable(GL_CULL_FACE);
//glCullFace(GL_BACK);
}
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);
}
void multiply(const NxQuat& left, const btVector3& right)
{
float a,b,c,d;
a = - left.x*right.x() - left.y*right.y() - left.z *right.z();
b = left.w*right.x() + left.y*right.z() - right.y()*left.z;
c = left.w*right.y() + left.z*right.x() - right.z()*left.x;
d = left.w*right.z() + left.x*right.y() - right.x()*left.y;
w = a;
x = b;
y = c;
z = d;
}
int RudeCollision::LineSegmentAndSphere(const btVector3 &p1, const btVector3 &p2, const btVector3 &sc, float r, btVector3 &int_p1, btVector3 &int_p2)
{
const float kEps = 0.00001f;
btVector3 dp = p2 - p1;
float a = dp.x() * dp.x() + dp.y() * dp.y() + dp.z() * dp.z();
float b = 2.0f * (dp.x() * (p1.x() - sc.x()) + dp.y() * (p1.y() - sc.y()) + dp.z() * (p1.z() - sc.z()));
float c = sc.x() * sc.x() + sc.y() * sc.y() + sc.z() * sc.z();
c += p1.x() * p1.x() + p1.y() * p1.y() + p1.z() * p1.z();
c -= 2 * (sc.x() * p1.x() + sc.y() * p1.y() + sc.z() * p1.z());
c -= r * r;
float bb4ac = b * b - 4 * a * c;
float absa = a > 0.0f ? a : -a;
if (absa < kEps || bb4ac < 0.0f)
return 0;
float abs_bb4ac = bb4ac ? bb4ac : -bb4ac;
if(abs_bb4ac < kEps)
{
float u = -b / (2.0f * a);
int_p1 = p1 + u * dp;
int_p2 = int_p1;
return 1;
}
int solns = 0;
float bb4acsqrt = sqrt(bb4ac);
float a2 = 2.0f * a;
float u1 = (-b + bb4acsqrt) / a2;
if(0.0f <= u1 && u1 <= 1.0f)
{
int_p1 = p1 + u1 * dp;
int_p2 = int_p1;
solns++;
}
float u2 = (-b - bb4acsqrt) / a2;
if(0.0f <= u2 && u2 <= 1.0f)
{
if(solns)
{
int_p2 = p1 + u2 * dp;
}
else
{
int_p1 = p1 + u2 * dp;
int_p2 = int_p2;
}
solns++;
}
return solns;
}