void b2LineJoint::SetMaxMotorForce(float32 force)
{
m_body1->WakeUp();
m_body2->WakeUp();
m_maxMotorForce = B2FORCE_SCALE(float32(1.0))*force;
}
void Timer::Initialize()
{
uint64 frequencyU64 = 0;
QueryPerformanceFrequency(PLARGE_INTEGER(&frequencyU64));
frequency = float32(frequencyU64);
}
bool Main::loop()
{
WindowDesc mainWindowDesc = WindowDesc::getDefault();
mainWindowDesc.title = cfgWindowTitle;
mainWindowDesc.width = cfgWindowWidth;
mainWindowDesc.height = cfgWindowHeight;
mainWindowDesc.posX = cfgWindowPosX;
mainWindowDesc.posY = cfgWindowPosY;
mainWindowDesc.debug_context = cfgDebugContext ? 1 : 0;
mainWindowDesc.forward_compat = cfgFwdCompatContext ? 1 : 0;
mainWindowDesc.major_version = cfgOpenglMajorVersion;
mainWindowDesc.minor_version = cfgOpenglMinorVersion;
mainWindowDesc.red_bits = cfgOpenglBackbufferRedBits;
mainWindowDesc.green_bits = cfgOpenglBackbufferGreenBits;
mainWindowDesc.blue_bits = cfgOpenglBackbufferBlueBits;
mainWindowDesc.alpha_bits = cfgOpenglBackbufferAlphaBits;
mainWindowDesc.depth_bits = cfgOpenglBackbufferDepthBits;
mainWindowDesc.stencil_bits = cfgOpenglBackbufferStencilBits;
mainWindowDesc.accum_bits = 0;
mainWindowDesc.aux_buffers = 0;
mainWindowDesc.samples = 1;
mainWindowDesc.refresh_rate = 0;
//create a window
Window * mainWindow = m_WindowSystem->openWindow(mainWindowDesc);
if (!mainWindow) return false;
//read back values
m_RenderSystem->attachWindow(mainWindow);
//
/// INPUT EXAMPLE
int32 inputIndex = m_InputSystem->attachWindow(mainWindow);
if (inputIndex == -1) return false;
InputDevice inputDevice;
inputDevice.connectDevice(m_InputSystem->getKeyboard());
inputDevice.connectDevice(m_InputSystem->getMouse());
//device.connectDevice(m_InputSystem->getJoystick());
/*
Trigger::ID trggrReturn = device.addTrigger(&Key::WentUp<Keyboard::Code::key_RETURN>);
Trigger::ID trggrLeftClick = device.addTrigger(&MouseButton::WentUp<Mouse::Button::Left>);
//Trigger::ID trggrXPressed = device.addTrigger(&JoystickButton::WentDown<Joystick::Button::Button1>);
Trigger::ID trggrConfigKey = device.addTrigger(&ConfigKey::WentDown<&cfgActionKey>);
Trigger::ID trggrChoord = device.addTrigger(
&And<
&Or<
&Key::IsPressed<Keyboard::Code::key_LSHIFT>,
&Key::IsPressed<Keyboard::Code::key_RSHIFT>
>,
&And<
&Key::IsReleased<Keyboard::Code::key_LCONTROL>,
&Key::IsReleased<Keyboard::Code::key_RCONTROL>
>,
&Key::WentDown<Keyboard::Code::key_A>
>);
*/
/*
trggrChoord = device.addTrigger(
&Map<
&Key::IsPressed<Keyboard::Code::key_LSHIFT>,
&Key::IsPressed<Keyboard::Code::key_RSHIFT>
>
);
//*/
///END INPUT EXAMPLE
//
/// build a simple game object
GameObject* obj = m_ObjectSystem->createObject();
obj->addComponent<InputWASDComponent>(inputDevice);
obj->setPosition(glm::vec3());
obj->lookInDirection(GameObject::FORWARD_DIRECTION, GameObject::UP_DIRECTION);
//create a camera and attach it to the object
Camera cam;
cam.attachToObject(obj);
cam.setViewportSize(glm::uvec2(cfgWindowWidth, cfgWindowHeight));
m_RenderSystem->getScene()->setCamera(&cam);
cam.update();
glm::mat4 viewMatrix = cam.getViewMatrix();
# if DEBUG_BUILD || SHOW_DEBUG_TITLE
const ansichar* dbg_WindowTitleTemplate = "DEBUG: CurrentFPS (%.3f)";
ansichar dbg_WindowTitle[128];
float32 accumDt = 0.0, lastTitleUpdate = 0.0;
# endif
typedef std::chrono::high_resolution_clock clock;
typedef clock::time_point time_point;
typedef clock::duration time_duration;
time_point start = clock::now(), current = start, last = start;
while (m_Running) {
current = clock::now();
float32 dt = float32(std::chrono::duration_cast<std::chrono::microseconds>(current - last).count());
float32 fps = CalcFPS(dt); //get the fps estimation
# if DEBUG_BUILD || SHOW_DEBUG_TITLE
accumDt += dt;
if (accumDt - lastTitleUpdate > oneSecond / 2) {
std::sprintf(dbg_WindowTitle, dbg_WindowTitleTemplate, fps);
lastTitleUpdate = accumDt;
mainWindow->setTitle(dbg_WindowTitle);
}
# endif
m_PackageSystem->tick(dt); //tick the package system (may reimport new packages)
if (!m_WindowSystem->tick(dt)) {
m_Running = false;
}
if (!m_InputSystem->tick(dt)) {
m_Running = false;
}
if(!m_ObjectSystem->tick(dt)){
m_Running = false;
}
if(!m_PhysicsSystem->tick(dt)){
m_Running = false;
}
if (!m_RenderSystem->tick(dt)) {
//Rendering failed -> shutdown
m_Running = false;
}
mainWindow->swapBuffers();
m_Running = m_Running && !mainWindow->isClosed();
last = current;
}
return false;
}
b2Vec2 b2MouseJoint::GetReactionForce() const
{
return B2FORCE_SCALE(float32(1.0))*m_impulse;
}
void Test::Step(Settings* settings)
{
float32 timeStep = settings->hz > 0.0f ? 1.0f / settings->hz : float32(0.0f);
if (settings->pause)
{
if (settings->singleStep)
{
settings->singleStep = 0;
}
else
{
timeStep = 0.0f;
}
m_debugDraw.DrawString(5, m_textLine, "****PAUSED****");
m_textLine += 15;
}
unsigned int flags = 0;
flags += settings->drawShapes * b2DebugDraw::e_shapeBit;
flags += settings->drawJoints * b2DebugDraw::e_jointBit;
flags += settings->drawAABBs * b2DebugDraw::e_aabbBit;
flags += settings->drawPairs * b2DebugDraw::e_pairBit;
flags += settings->drawCOMs * b2DebugDraw::e_centerOfMassBit;
m_debugDraw.SetFlags(flags);
m_world->SetWarmStarting(settings->enableWarmStarting > 0);
m_world->SetContinuousPhysics(settings->enableContinuous > 0);
m_pointCount = 0;
m_world->Step(timeStep, settings->velocityIterations, settings->positionIterations);
if (timeStep > 0.0f)
{
++m_stepCount;
}
if (settings->drawStats)
{
m_debugDraw.DrawString(5, m_textLine, "bodies/contacts/joints/proxies = %d/%d/%d",
m_world->GetBodyCount(), m_world->GetContactCount(), m_world->GetJointCount(), m_world->GetProxyCount());
m_textLine += 15;
}
if (m_mouseJoint)
{
//b2Body* body = m_mouseJoint->GetBody2();
//b2Vec2 p1 = body->GetWorldPoint(m_mouseJoint->m_localAnchor);
//b2Vec2 p2 = m_mouseJoint->m_target;
// glPointSize(4.0f);
// glColor3f(0.0f, 1.0f, 0.0f);
// glBegin(GL_POINTS);
// glVertex2f(p1.x, p1.y);
// glVertex2f(p2.x, p2.y);
// glEnd();
// glPointSize(1.0f);
//
// glColor3f(0.8f, 0.8f, 0.8f);
// glBegin(GL_LINES);
// glVertex2f(p1.x, p1.y);
// glVertex2f(p2.x, p2.y);
// glEnd();
}
if (m_bombSpawning)
{
// glPointSize(4.0f);
// glColor3f(0.0f, 0.0f, 1.0f);
// glBegin(GL_POINTS);
// glColor3f(0.0f, 0.0f, 1.0f);
// glVertex2f(m_bombSpawnPoint.x, m_bombSpawnPoint.y);
// glEnd();
//
// glColor3f(0.8f, 0.8f, 0.8f);
// glBegin(GL_LINES);
// glVertex2f(m_mouseWorld.x, m_mouseWorld.y);
// glVertex2f(m_bombSpawnPoint.x, m_bombSpawnPoint.y);
// glEnd();
}
if (settings->drawContactPoints)
{
//const float32 k_impulseScale = 0.1f;
const float32 k_axisScale = 0.3f;
for (int i = 0; i < m_pointCount; ++i)
{
ContactPoint* point = m_points + i;
if (point->state == b2_addState)
{
// Add
m_debugDraw.DrawPoint(point->position, 10.0f, b2Color(0.3f, 0.95f, 0.3f));
}
else if (point->state == b2_persistState)
{
// Persist
m_debugDraw.DrawPoint(point->position, 5.0f, b2Color(0.3f, 0.3f, 0.95f));
}
if (settings->drawContactNormals == 1)
{
b2Vec2 p1 = point->position;
b2Vec2 p2 = p1 + k_axisScale * point->normal;
m_debugDraw.DrawSegment(p1, p2, b2Color(0.4f, 0.9f, 0.4f));
}
else if (settings->drawContactForces == 1)
{
//b2Vec2 p1 = point->position;
//b2Vec2 p2 = p1 + k_forceScale * point->normalForce * point->normal;
//DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
}
if (settings->drawFrictionForces == 1)
{
//b2Vec2 tangent = b2Cross(point->normal, 1.0f);
//b2Vec2 p1 = point->position;
//b2Vec2 p2 = p1 + k_forceScale * point->tangentForce * tangent;
//DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
}
}
}
}
void b3HullShape::ComputeMass(b3MassData* massData, float32 density) const
{
// M. Kallay - "Computing the Moment of Inertia of a Solid Defined by a Triangle Mesh"
// https://github.com/erich666/jgt-code/blob/master/Volume_11/Number_2/Kallay2006/Moment_of_Inertia.cpp
// Polyhedron mass, center of mass, and inertia.
// Let rho be the polyhedron density per unit volume
// mass = rho * int(1 * dV)
// centroid.x = (1 / mass) * rho * int(x * dV)
// centroid.y = (1 / mass) * rho * int(y * dV)
// centroid.z = (1 / mass) * rho * int(z * dV)
// Ixx = rho * int((y^2 + z^2) * dV)
// Iyy = rho * int((x^2 + z^2) * dV)
// Izz = rho * int((x^2 + y^2) * dV)
// Ixy = -rho * int((x * y) * dV)
// Ixz = -rho * int((x * z) * dV)
// Iyz = -rho * int((y * z) * dV)
// Iyx = Ixy
// Izx = Ixz
// Izy = Iyz
B3_ASSERT(m_hull->vertexCount >= 4);
// Put the hull relative to a point that is inside the hull
// to help reducing round-off errors.
b3Vec3 s; s.SetZero();
for (u32 i = 0; i < m_hull->vertexCount; ++i)
{
s += m_hull->vertices[i];
}
s /= float32(m_hull->vertexCount);
float32 volume = 0.0f;
b3Vec3 center; center.SetZero();
float32 xx = 0.0f;
float32 xy = 0.0f;
float32 yy = 0.0f;
float32 xz = 0.0f;
float32 zz = 0.0f;
float32 yz = 0.0f;
for (u32 i = 0; i < m_hull->faceCount; ++i)
{
const b3Face* face = m_hull->GetFace(i);
const b3HalfEdge* begin = m_hull->GetEdge(face->edge);
const b3HalfEdge* edge = m_hull->GetEdge(begin->next);
do
{
const b3HalfEdge* next = m_hull->GetEdge(edge->next);
u32 i1 = begin->origin;
u32 i2 = edge->origin;
u32 i3 = next->origin;
b3Vec3 v1 = m_hull->GetVertex(i1) - s;
b3Vec3 v2 = m_hull->GetVertex(i2) - s;
b3Vec3 v3 = m_hull->GetVertex(i3) - s;
// Signed tetrahedron volume
float32 D = b3Det(v1, v2, v3);
// Contribution to the mass
volume += D;
// Contribution to the centroid
b3Vec3 v4 = v1 + v2 + v3;
center += D * v4;
// Contribution to moment of inertia monomials
xx += D * (v1.x * v1.x + v2.x * v2.x + v3.x * v3.x + v4.x * v4.x);
yy += D * (v1.y * v1.y + v2.y * v2.y + v3.y * v3.y + v4.y * v4.y);
zz += D * (v1.z * v1.z + v2.z * v2.z + v3.z * v3.z + v4.z * v4.z);
xy += D * (v1.x * v1.y + v2.x * v2.y + v3.x * v3.y + v4.x * v4.y);
xz += D * (v1.x * v1.z + v2.x * v2.z + v3.x * v3.z + v4.x * v4.z);
yz += D * (v1.y * v1.z + v2.y * v2.z + v3.y * v3.z + v4.y * v4.z);
edge = next;
} while (m_hull->GetEdge(edge->next) != begin);
}
b3Mat33 I;
I.x.x = yy + zz;
I.x.y = -xy;
I.x.z = -xz;
I.y.x = -xy;
I.y.y = xx + zz;
I.y.z = -yz;
I.z.x = -xz;
I.z.y = -yz;
I.z.z = xx + yy;
// Total mass
massData->mass = density * volume / 6.0f;
// Center of mass
B3_ASSERT(volume > B3_EPSILON);
center /= 4.0f * volume;
massData->center = center + s;
// Inertia relative to the local origin (s).
massData->I = (density / 120.0f) * I;
// Shift the inertia to center of mass then to the body origin.
// Ib = Ic - m * c^2 + m * m.c^2
// Simplification:
// Ib = Ic + m * (m.c^2 - c^2)
massData->I += massData->mass * (b3Steiner(massData->center) - b3Steiner(center));
}
void Test::Step(Settings* settings)
{
float32 timeStep = settings->hz > 0.0f ? 1.0f / settings->hz : float32(0.0f);
if (settings->pause)
{
if (settings->singleStep)
{
settings->singleStep = 0;
}
else
{
timeStep = 0.0f;
}
m_debugDraw.DrawString(5, m_textLine, "****PAUSED****");
m_textLine += DRAW_STRING_NEW_LINE;
}
uint32 flags = 0;
flags += settings->drawShapes * b2Draw::e_shapeBit;
flags += settings->drawParticles * b2Draw::e_particleBit;
flags += settings->drawJoints * b2Draw::e_jointBit;
flags += settings->drawAABBs * b2Draw::e_aabbBit;
flags += settings->drawCOMs * b2Draw::e_centerOfMassBit;
m_debugDraw.SetFlags(flags);
m_world->SetAllowSleeping(settings->enableSleep > 0);
m_world->SetWarmStarting(settings->enableWarmStarting > 0);
m_world->SetContinuousPhysics(settings->enableContinuous > 0);
m_world->SetSubStepping(settings->enableSubStepping > 0);
m_particleSystem->SetStrictContactCheck(settings->strictContacts > 0);
m_pointCount = 0;
b2Timer timer;
m_world->Step(timeStep,
settings->velocityIterations,
settings->positionIterations,
settings->particleIterations);
settings->stepTimeOut = timer.GetMilliseconds();
m_world->DrawDebugData();
if (timeStep > 0.0f)
{
++m_stepCount;
}
if (settings->drawStats)
{
int32 bodyCount = m_world->GetBodyCount();
int32 contactCount = m_world->GetContactCount();
int32 jointCount = m_world->GetJointCount();
m_debugDraw.DrawString(5, m_textLine, "bodies/contacts/joints = %d/%d/%d", bodyCount, contactCount, jointCount);
m_textLine += DRAW_STRING_NEW_LINE;
int32 particleCount = m_particleSystem->GetParticleCount();
int32 groupCount = m_particleSystem->GetParticleGroupCount();
int32 pairCount = m_particleSystem->GetPairCount();
int32 triadCount = m_particleSystem->GetTriadCount();
m_debugDraw.DrawString(5, m_textLine, "particles/groups/pairs/triads = %d/%d/%d/%d", particleCount, groupCount, pairCount, triadCount);
m_textLine += DRAW_STRING_NEW_LINE;
int32 proxyCount = m_world->GetProxyCount();
int32 height = m_world->GetTreeHeight();
int32 balance = m_world->GetTreeBalance();
float32 quality = m_world->GetTreeQuality();
m_debugDraw.DrawString(5, m_textLine, "proxies/height/balance/quality = %d/%d/%d/%g", proxyCount, height, balance, quality);
m_textLine += DRAW_STRING_NEW_LINE;
}
// Track maximum profile times
{
const b2Profile& p = m_world->GetProfile();
m_maxProfile.step = b2Max(m_maxProfile.step, p.step);
m_maxProfile.collide = b2Max(m_maxProfile.collide, p.collide);
m_maxProfile.solve = b2Max(m_maxProfile.solve, p.solve);
m_maxProfile.solveInit = b2Max(m_maxProfile.solveInit, p.solveInit);
m_maxProfile.solveVelocity = b2Max(m_maxProfile.solveVelocity, p.solveVelocity);
m_maxProfile.solvePosition = b2Max(m_maxProfile.solvePosition, p.solvePosition);
m_maxProfile.solveTOI = b2Max(m_maxProfile.solveTOI, p.solveTOI);
m_maxProfile.broadphase = b2Max(m_maxProfile.broadphase, p.broadphase);
m_totalProfile.step += p.step;
m_totalProfile.collide += p.collide;
m_totalProfile.solve += p.solve;
m_totalProfile.solveInit += p.solveInit;
m_totalProfile.solveVelocity += p.solveVelocity;
m_totalProfile.solvePosition += p.solvePosition;
m_totalProfile.solveTOI += p.solveTOI;
m_totalProfile.broadphase += p.broadphase;
}
if (settings->drawProfile)
{
const b2Profile& p = m_world->GetProfile();
b2Profile aveProfile;
memset(&aveProfile, 0, sizeof(b2Profile));
if (m_stepCount > 0)
{
float32 scale = 1.0f / m_stepCount;
aveProfile.step = scale * m_totalProfile.step;
aveProfile.collide = scale * m_totalProfile.collide;
aveProfile.solve = scale * m_totalProfile.solve;
aveProfile.solveInit = scale * m_totalProfile.solveInit;
aveProfile.solveVelocity = scale * m_totalProfile.solveVelocity;
aveProfile.solvePosition = scale * m_totalProfile.solvePosition;
aveProfile.solveTOI = scale * m_totalProfile.solveTOI;
aveProfile.broadphase = scale * m_totalProfile.broadphase;
}
m_debugDraw.DrawString(5, m_textLine, "step [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.step, aveProfile.step, m_maxProfile.step);
m_textLine += DRAW_STRING_NEW_LINE;
m_debugDraw.DrawString(5, m_textLine, "collide [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.collide, aveProfile.collide, m_maxProfile.collide);
m_textLine += DRAW_STRING_NEW_LINE;
m_debugDraw.DrawString(5, m_textLine, "solve [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solve, aveProfile.solve, m_maxProfile.solve);
m_textLine += DRAW_STRING_NEW_LINE;
m_debugDraw.DrawString(5, m_textLine, "solve init [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveInit, aveProfile.solveInit, m_maxProfile.solveInit);
m_textLine += DRAW_STRING_NEW_LINE;
m_debugDraw.DrawString(5, m_textLine, "solve velocity [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveVelocity, aveProfile.solveVelocity, m_maxProfile.solveVelocity);
m_textLine += DRAW_STRING_NEW_LINE;
m_debugDraw.DrawString(5, m_textLine, "solve position [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solvePosition, aveProfile.solvePosition, m_maxProfile.solvePosition);
m_textLine += DRAW_STRING_NEW_LINE;
m_debugDraw.DrawString(5, m_textLine, "solveTOI [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.solveTOI, aveProfile.solveTOI, m_maxProfile.solveTOI);
m_textLine += DRAW_STRING_NEW_LINE;
m_debugDraw.DrawString(5, m_textLine, "broad-phase [ave] (max) = %5.2f [%6.2f] (%6.2f)", p.broadphase, aveProfile.broadphase, m_maxProfile.broadphase);
m_textLine += DRAW_STRING_NEW_LINE;
}
if (m_mouseTracing && !m_mouseJoint)
{
float32 delay = 0.1f;
b2Vec2 acceleration = 2 / delay * (1 / delay * (m_mouseWorld - m_mouseTracerPosition) - m_mouseTracerVelocity);
m_mouseTracerVelocity += timeStep * acceleration;
m_mouseTracerPosition += timeStep * m_mouseTracerVelocity;
b2CircleShape shape;
shape.m_p = m_mouseTracerPosition;
shape.m_radius = 2 * GetDefaultViewZoom();
QueryCallback2 callback(m_particleSystem, &shape, m_mouseTracerVelocity);
b2AABB aabb;
b2Transform xf;
xf.SetIdentity();
shape.ComputeAABB(&aabb, xf, 0);
m_world->QueryAABB(&callback, aabb);
}
if (m_mouseJoint)
{
b2Vec2 p1 = m_mouseJoint->GetAnchorB();
b2Vec2 p2 = m_mouseJoint->GetTarget();
b2Color c;
c.Set(0.0f, 1.0f, 0.0f);
m_debugDraw.DrawPoint(p1, 4.0f, c);
m_debugDraw.DrawPoint(p2, 4.0f, c);
c.Set(0.8f, 0.8f, 0.8f);
m_debugDraw.DrawSegment(p1, p2, c);
}
if (m_bombSpawning)
{
b2Color c;
c.Set(0.0f, 0.0f, 1.0f);
m_debugDraw.DrawPoint(m_bombSpawnPoint, 4.0f, c);
c.Set(0.8f, 0.8f, 0.8f);
m_debugDraw.DrawSegment(m_mouseWorld, m_bombSpawnPoint, c);
}
if (settings->drawContactPoints)
{
const float32 k_impulseScale = 0.1f;
const float32 k_axisScale = 0.3f;
for (int32 i = 0; i < m_pointCount; ++i)
{
ContactPoint* point = m_points + i;
if (point->state == b2_addState)
{
// Add
m_debugDraw.DrawPoint(point->position, 10.0f, b2Color(0.3f, 0.95f, 0.3f));
}
else if (point->state == b2_persistState)
{
// Persist
m_debugDraw.DrawPoint(point->position, 5.0f, b2Color(0.3f, 0.3f, 0.95f));
}
if (settings->drawContactNormals)
{
b2Vec2 p1 = point->position;
b2Vec2 p2 = p1 + k_axisScale * point->normal;
m_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.9f));
}
else if (settings->drawContactImpulse)
{
b2Vec2 p1 = point->position;
b2Vec2 p2 = p1 + k_impulseScale * point->normalImpulse * point->normal;
m_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
}
if (settings->drawFrictionImpulse)
{
b2Vec2 tangent = b2Cross(point->normal, 1.0f);
b2Vec2 p1 = point->position;
b2Vec2 p2 = p1 + k_impulseScale * point->tangentImpulse * tangent;
m_debugDraw.DrawSegment(p1, p2, b2Color(0.9f, 0.9f, 0.3f));
}
}
}
}
