fix capsule drawing, add quaternion convenience functions
This commit is contained in:
Irlan
parent
72d8815a38
commit
875cc5316b
@ -77,18 +77,6 @@ struct b3Quat
|
||||
w = _w;
|
||||
}
|
||||
|
||||
// Set this quaternion from an axis and an angle
|
||||
// of rotation about the axis.
|
||||
void Set(const b3Vec3& axis, float32 angle)
|
||||
{
|
||||
float32 theta = 0.5f * angle;
|
||||
float32 s = sin(theta);
|
||||
x = s * axis.x;
|
||||
y = s * axis.y;
|
||||
z = s * axis.z;
|
||||
w = cos(theta);
|
||||
}
|
||||
|
||||
// Normalize this quaternion.
|
||||
void Normalize()
|
||||
{
|
||||
@ -102,6 +90,54 @@ struct b3Quat
|
||||
}
|
||||
}
|
||||
|
||||
// Set this quaternion from an axis and full angle
|
||||
// of rotation about the axis.
|
||||
void Set(const b3Vec3& axis, float32 angle)
|
||||
{
|
||||
// half angle
|
||||
float32 theta = 0.5f * angle;
|
||||
|
||||
float32 sine = sin(theta);
|
||||
x = sine * axis.x;
|
||||
y = sine * axis.y;
|
||||
z = sine * axis.z;
|
||||
|
||||
w = cos(theta);
|
||||
}
|
||||
|
||||
// If this quaternion represents an orientation return
|
||||
// the axis of rotation.
|
||||
b3Vec3 GetAxis() const
|
||||
{
|
||||
// sin^2 = 1 - cos^2
|
||||
// sin = sqrt( sin^2 ) = ||v||
|
||||
// axis = v / sin
|
||||
b3Vec3 v(x, y, z);
|
||||
float32 sine = b3Length(v);
|
||||
if (sine > B3_EPSILON)
|
||||
{
|
||||
float32 s = 1.0f / sine;
|
||||
return s * v;
|
||||
}
|
||||
return v;
|
||||
}
|
||||
|
||||
// If this quaternion represents an orientation return
|
||||
// the full angle of rotation.
|
||||
float32 GetAngle() const
|
||||
{
|
||||
// cosine check
|
||||
if (w >= -1.0f && w <= 1.0f)
|
||||
{
|
||||
// half angle
|
||||
float32 theta = acos(w);
|
||||
// full angle
|
||||
return 2.0f * theta;
|
||||
}
|
||||
|
||||
return 0.0f;
|
||||
}
|
||||
|
||||
float32 x, y, z, w;
|
||||
};
|
||||
|
||||
|
||||
@ -22,8 +22,15 @@
|
||||
class CapsuleStack : public Test
|
||||
{
|
||||
public:
|
||||
enum
|
||||
{
|
||||
e_rowCount = 5,
|
||||
e_columnCount = 5,
|
||||
e_depthCount = 5
|
||||
};
|
||||
|
||||
CapsuleStack()
|
||||
{
|
||||
{
|
||||
{
|
||||
b3BodyDef bd;
|
||||
bd.type = b3BodyType::e_staticBody;
|
||||
@ -35,34 +42,76 @@ public:
|
||||
|
||||
b3ShapeDef sd;
|
||||
sd.shape = &hs;
|
||||
sd.density = 0.0f;
|
||||
sd.friction = 1.0f;
|
||||
sd.restitution = 0.0f;
|
||||
|
||||
b3Shape* shape = body->CreateShape(sd);
|
||||
body->CreateShape(sd);
|
||||
}
|
||||
|
||||
for (float32 y = 2.5f; y < 20.0f; y += 2.5f)
|
||||
|
||||
float32 height = 3.0f;
|
||||
float32 radius = 1.0f;
|
||||
float32 separation = 0.0f;
|
||||
|
||||
b3CapsuleShape capsule;
|
||||
capsule.m_centers[0].Set(0.0f, -0.5f * height, 0.0f);
|
||||
capsule.m_centers[1].Set(0.0f, 0.5f * height, 0.0f);
|
||||
capsule.m_radius = radius;
|
||||
|
||||
b3ShapeDef sdef;
|
||||
sdef.shape = &capsule;
|
||||
sdef.density = 1.0f;
|
||||
sdef.friction = 0.3f;
|
||||
|
||||
const u32 c = e_rowCount * e_columnCount * e_depthCount;
|
||||
b3Body* bs[c];
|
||||
u32 n = 0;
|
||||
|
||||
b3AABB3 aabb;
|
||||
aabb.m_lower.Set(0.0f, 0.0f, 0.0f);
|
||||
aabb.m_upper.Set(0.0f, 0.0f, 0.0f);
|
||||
|
||||
for (u32 i = 0; i < e_rowCount; ++i)
|
||||
{
|
||||
b3BodyDef bdef;
|
||||
bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
|
||||
bdef.position.Set(0.0f, y, 0.0f);
|
||||
bdef.linearVelocity.Set(0.0f, -1.0f, 0.0f);
|
||||
bdef.type = b3BodyType::e_dynamicBody;
|
||||
for (u32 j = 0; j < e_columnCount; ++j)
|
||||
{
|
||||
for (u32 k = 0; k < e_depthCount; ++k)
|
||||
{
|
||||
b3BodyDef bdef;
|
||||
bdef.type = b3BodyType::e_dynamicBody;
|
||||
|
||||
bdef.position.x = (2.0f + separation) * float32(i) * (0.5f * height + radius);
|
||||
bdef.position.y = (2.0f + separation) * float32(j) * radius;
|
||||
bdef.position.z = (2.0f + separation) * float32(k) * radius;
|
||||
|
||||
bdef.orientation = b3Quat(b3Vec3(0.0f, 0.0f, 1.0f), 0.5f * B3_PI);
|
||||
|
||||
b3Body* body = m_world.CreateBody(bdef);
|
||||
bs[n++] = body;
|
||||
|
||||
b3Body* body = m_world.CreateBody(bdef);
|
||||
b3Shape* shape = body->CreateShape(sdef);
|
||||
|
||||
b3CapsuleShape capsule;
|
||||
capsule.m_centers[0].Set(0.0f, -1.0f, 0.0f);
|
||||
capsule.m_centers[1].Set(0.0f, 1.0f, 0.0f);
|
||||
capsule.m_radius = 1.0f;
|
||||
b3AABB3 aabb2;
|
||||
shape->ComputeAABB(&aabb2, body->GetTransform());
|
||||
|
||||
b3ShapeDef sdef;
|
||||
sdef.shape = &capsule;
|
||||
sdef.density = 1.0f;
|
||||
sdef.friction = 0.3f;
|
||||
aabb = b3Combine(aabb, aabb2);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
b3Shape* shape = body->CreateShape(sdef);
|
||||
b3Vec3 center = aabb.Centroid();
|
||||
|
||||
for (u32 i = 0; i < n; ++i)
|
||||
{
|
||||
b3Body* b = bs[i];
|
||||
const b3Vec3& p = b->GetSweep().worldCenter;
|
||||
const b3Quat& q = b->GetSweep().orientation;
|
||||
|
||||
// centralize
|
||||
b3Vec3 position = p - center;
|
||||
|
||||
// move up
|
||||
position.y += 0.5f * aabb.Height() + radius;
|
||||
|
||||
// maintain orientation
|
||||
b->SetTransform(position, q.GetAxis(), q.GetAngle());
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
@ -1295,7 +1295,10 @@ void DebugDraw::DrawString(const char* text, const b3Color& color, ...)
|
||||
|
||||
void DebugDraw::DrawSphere(const b3SphereShape* s, const b3Color& c, const b3Transform& xf)
|
||||
{
|
||||
m_solid->DrawSphere(s->m_radius, c, xf);
|
||||
b3Transform xfc;
|
||||
xfc.rotation = xf.rotation;
|
||||
xfc.position = xf * s->m_center;
|
||||
m_solid->DrawSphere(s->m_radius, c, xfc);
|
||||
}
|
||||
|
||||
void DebugDraw::DrawCapsule(const b3CapsuleShape* s, const b3Color& c, const b3Transform& xf)
|
||||
@ -1317,14 +1320,15 @@ void DebugDraw::DrawCapsule(const b3CapsuleShape* s, const b3Color& c, const b3T
|
||||
{
|
||||
b3Transform xfc;
|
||||
xfc.rotation = xf.rotation;
|
||||
xfc.position = xf * c2;
|
||||
m_solid->DrawSphere(radius, c, xfc);
|
||||
xfc.position = xf * ( 0.5f * (c1 + c2) );
|
||||
m_solid->DrawCylinder(radius, height, c, xfc);
|
||||
}
|
||||
|
||||
{
|
||||
const float32 z_bias = 0.0002f;
|
||||
radius += z_bias;
|
||||
m_solid->DrawCylinder(radius, height, c, xf);
|
||||
b3Transform xfc;
|
||||
xfc.rotation = xf.rotation;
|
||||
xfc.position = xf * c2;
|
||||
m_solid->DrawSphere(radius, c, xfc);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -222,8 +222,10 @@ bool GetTestName(void*, int idx, const char** name)
|
||||
|
||||
void Interface()
|
||||
{
|
||||
ImGui::SetNextWindowPos(ImVec2(g_camera.m_width - 250.0f, 0.0f));
|
||||
ImGui::SetNextWindowPos(ImVec2(g_camera.m_width, 0.0f));
|
||||
ImGui::SetNextWindowSize(ImVec2(250.0f, g_camera.m_height));
|
||||
ImGui::PushStyleVar(ImGuiStyleVar_WindowRounding, 0.0f);
|
||||
|
||||
ImGui::Begin("Controls", NULL, ImVec2(0.0f, 0.0f), 0.25f, ImGuiWindowFlags_NoMove | ImGuiWindowFlags_NoResize | ImGuiWindowFlags_NoCollapse);
|
||||
|
||||
ImGui::PushItemWidth(-1.0f);
|
||||
@ -296,6 +298,7 @@ void Interface()
|
||||
ImGui::Checkbox("Profile", &g_settings.drawProfile);
|
||||
|
||||
ImGui::End();
|
||||
ImGui::PopStyleVar();
|
||||
}
|
||||
|
||||
void Step()
|
||||
@ -361,10 +364,10 @@ void Run()
|
||||
{
|
||||
int width, height;
|
||||
glfwGetWindowSize(g_window, &width, &height);
|
||||
g_camera.m_width = float32(width);
|
||||
g_camera.m_width = float32(width) - 250.0f;
|
||||
g_camera.m_height = float32(height);
|
||||
|
||||
glViewport(0, 0, width, height);
|
||||
glViewport(0, 0, width - 250, height);
|
||||
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
|
||||
|
||||
ImGui_ImplGlfwGL3_NewFrame();
|
||||
|
||||
@ -387,6 +387,11 @@ void Test::Step()
|
||||
ImGui::SetNextWindowPos(ImVec2(0.0f, 0.0f));
|
||||
ImGui::Begin("Log", NULL, ImVec2(0, 0), 0.0f, ImGuiWindowFlags_NoTitleBar | ImGuiWindowFlags_NoInputs | ImGuiWindowFlags_AlwaysAutoResize | ImGuiWindowFlags_NoScrollbar);
|
||||
|
||||
if (g_settings.pause)
|
||||
{
|
||||
ImGui::Text("*PAUSED*");
|
||||
}
|
||||
|
||||
if (g_settings.drawStats)
|
||||
{
|
||||
ImGui::Text("Bodies %d", m_world.GetBodyList().m_count);
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user