void Graphics::PlotTriangle(HDC hdc, const TriangleSelfContained& triangle) {
ScreenPoint points[3] = { triangle[0], triangle[1], triangle[2] };
if ((Approximately(points[0].x, points[1].x) && Approximately(points[1].x, points[2].x))
|| (Approximately(points[0].y, points[1].y) && Approximately(points[1].y, points[2].y)))
return;
SortPoints(points);
if (Approximately(points[0].y, points[1].y)) {
PlotFlatTopTriangle(hdc, points[0], points[1], points[2], triangle.GetTexture());
}
else if (Approximately(points[1].y, points[2].y)) {
PlotFlatBottomTriangle(hdc, points[1], points[2], points[0], triangle.GetTexture());
}
else {
float tx = points[0].x + (points[1].y - points[0].y) * (points[2].x - points[0].x) / float(points[2].y - points[0].y);
Color color = (points[0].color + (points[2].color - points[0].color) * ((points[1].y - points[0].y) / float(points[2].y - points[0].y)));
float tz = points[0].z + (points[1].y - points[0].y) * (points[2].z - points[0].z) / float(points[2].y - points[0].y);
float tu = points[0].u + (points[1].y - points[0].y) * (points[2].u - points[0].u) / float(points[2].y - points[0].y);
float tv = points[0].v + (points[1].y - points[0].y) * (points[2].v - points[0].v) / float(points[2].y - points[0].y);
ScreenPoint tp(tx, points[1].y, tz, tu, tv, color);
PlotFlatBottomTriangle(hdc, tp, points[1], points[0], triangle.GetTexture());
PlotFlatTopTriangle(hdc, points[1], tp, points[2], triangle.GetTexture());
}
}
void RigidBodyContact::ApplyVelocityChange(Vector3* velocityChange, Vector3* angularVelocityChange)
{
// Get inverse mass and inverse inertia tensor, in world coords
Matrix3x3 inverseInertiaTensor[2];
inverseInertiaTensor[0] = Body[0]->GetInverseIntertiaTensorWorld();
if (Body[1] != NULL)
{
inverseInertiaTensor[1] = Body[1]->GetInverseIntertiaTensorWorld();
}
// We'll need to calculate the impulse for each contact axis
Vector3 impulseContactCoords;
if (Approximately(Friction, 0.0f))
{
// Friction is effectively zero, so do (simpler) frictionless calculation
impulseContactCoords = CalculateFrictionlessImpulse(inverseInertiaTensor);
}
else
{
impulseContactCoords = CalculateFrictionImpulse(inverseInertiaTensor);
}
// Convert impulse to world coords
Vector3 impulseWorldCoords = m_contactToWorld * impulseContactCoords;
// Split the impulse into linear and angular components
Vector3 impulsiveTorque = m_relativeContactPosition[0].Cross(impulseWorldCoords);
angularVelocityChange[0] = inverseInertiaTensor[0] * impulsiveTorque;
velocityChange[0] = impulseWorldCoords * Body[0]->GetInverseMass();
Body[0]->AddVelocity(velocityChange[0]);
Body[0]->AddAngularVelocity(angularVelocityChange[0]);
if (Body[1] != NULL)
{
impulsiveTorque = impulseWorldCoords.Cross(m_relativeContactPosition[1]);
angularVelocityChange[1] = inverseInertiaTensor[1] * impulsiveTorque;
velocityChange[1] = impulseWorldCoords * (-1) * Body[1]->GetInverseMass();
Body[1]->AddVelocity(velocityChange[1]);
Body[1]->AddAngularVelocity(angularVelocityChange[1]);
}
}
void Graphics::PlotFlatBottomTriangle(HDC hdc, ScreenPoint p1, ScreenPoint p2, ScreenPoint p3, const Texture& texture) {
AssertEx(Approximately(p1.y, p2.y) && p1.y >= p3.y, "invalid flat bottom triangle");
if (p1.x > p2.x) std::swap(p1, p2);
float dy = -float(p3.y - p1.y);
float dxdyl = float(p1.x - p3.x) / dy;
float dxdyr = float(p2.x - p3.x) / dy;
ColorDiff dcdyl = (p1.color - p3.color) / dy;
ColorDiff dcdyr = (p2.color - p3.color) / dy;
float dzdyl = float(1.f / p1.z - 1.f / p3.z) / dy;
float dzdyr = float(1.f / p2.z - 1.f / p3.z) / dy;
float dudyl = float(p1.u / p1.z - p3.u / p3.z) / dy;
float dudyr = float(p2.u / p2.z - p3.u / p3.z) / dy;
float dvdyl = float(p1.v / p1.z - p3.v / p3.z) / dy;
float dvdyr = float(p2.v / p2.z - p3.v / p3.z) / dy;
float xl = (float)p3.x, xr = (float)p3.x;
float zl = 1.f / p3.z, zr = 1.f / p3.z;
Color cl = p3.color;
Color cr = p3.color;
float ul = (float)p3.u / p3.z;
float vl = (float)p3.v / p3.z;
float ur = (float)p3.u / p3.z;
float vr = (float)p3.v / p3.z;
int iy1 = 0, iy3 = 0;
if (p3.y < ymin) {
UPDATE_VAR(ymin - p3.y);
p3.y = ymin;
iy3 = p3.y;
}
else {
iy3 = (int)ceilf(p3.y);
UPDATE_VAR(iy3 - p3.y);
}
if (p1.y > ymax) {
p1.y = ymax;
iy1 = p1.y - 1;
}
else {
iy1 = ceilf(p1.y) - 1;
}
if (Between(p1.x, xmin, xmax) && Between(p2.x, xmin, xmax) && Between(p3.x, xmin, xmax)) {
for (int y = iy3; y <= iy1; ++y) {
PlotGradientLine(hdc, cl, cr, zl, zr, ul, ur, vl, vr, xl, xr, y, texture);
UPDATE_VAR(1.f);
}
}
else {
for (int y = iy3; y <= iy1; ++y) {
float left = xl, right = xr;
Color tcl = cl;
float tzl = zl, tul = ul, tvl = vl;
if(right >= xmin && left <= xmax) {
if (left < xmin) {
SHIFT_RIGHT_SCALE((xmin - left) / (right - left));
left = xmin;
}
right = Min(right, xmax);
PlotGradientLine(hdc, tcl, cr, tzl, zr, tul, ur, tvl, vr, left, right, y, texture);
}
UPDATE_VAR(1.f);
}
}
}
void RigidBodyContact::ApplyPositionChange(Vector3* linearChange, Vector3* angularChange, float penetration)
{
float angularLimit = 5.0f;
float angularMove[2];
float linearMove[2];
float linearInertia[2];
float angularInteria[2];
float totalInertia = 0.0f;
// Calculate the angular inertia using the same process as for calculating frictionless velocity change
for (int i = 0; i < 2; i++)
{
if (Body[i] != NULL)
{
Matrix3x3 inverseInertiaTensor = Body[i]->GetInverseIntertiaTensorWorld();
Vector3 angularInertiaWorld = m_relativeContactPosition[i].Cross(ContactNormal);
angularInertiaWorld = angularInertiaWorld*inverseInertiaTensor; // TODO physics: is order correct here?
angularInertiaWorld = angularInertiaWorld.Cross(m_relativeContactPosition[i]);
angularInteria[i] = angularInertiaWorld.Dot(ContactNormal);
// The linear inertia component is just the inverse mass
linearInertia[i] = Body[i]->GetInverseMass();
// Combine angular and linear to calculate total
totalInertia += linearInertia[i] + angularInteria[i];
}
}
// Calculate and apply changes
for (int i = 0; i < 2; i++)
{
if (Body[i] != NULL)
{
// The linear and angular movements required are in proportion to the two inverse inertias
float sign = (i == 0) ? 1.f : -1.f;
angularMove[i] = sign * penetration * (angularInteria[i] / totalInertia);
linearMove[i] = sign * penetration * (linearInertia[i] / totalInertia);
// To avoid angular projections that are too great (when mass is large but inertia
// tensor is small), limit the angular move
Vector3 projection = m_relativeContactPosition[i];
projection += ContactNormal * -m_relativeContactPosition[i].Dot(ContactNormal);
// Use the small angle approximation for the sine of the angle (i.e. the magnitude would be
// sin(angularLimit) * projection.magnitude, but we approximate sin(angularLimit) to angularLimit.
float maxMagnitude = angularLimit * projection.Magnitude();
if (angularMove[i] < -maxMagnitude)
{
float totalMove = angularMove[i] + linearMove[i];
angularMove[i] = -maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
}
else if (angularMove[i] > maxMagnitude)
{
float totalMove = angularMove[i] + linearMove[i];
angularMove[i] = maxMagnitude;
linearMove[i] = totalMove - angularMove[i];
}
// We have the linear amount of movement required by turning the rigid body (angularMove[i]).
// We now need to calculate the desired rotation to achieve that.
if (Approximately(angularMove[i], 0.f))
{
// Easy case - no angular movement means no rotation.
angularChange[i] = Vector3::Zero;
}
else
{
// Work out the direction we'd like to rotate in
Vector3 targetAngularDirection = m_relativeContactPosition[i].Cross(ContactNormal); // TODO physics: should this get normalized?
// Work out the direction we'd need to rotate to achieve that
Matrix3x3 inverseInertiaTensor = Body[i]->GetInverseIntertiaTensorWorld();
angularChange[i] = (targetAngularDirection*inverseInertiaTensor)* (angularMove[i] / angularInteria[i]); // TODO physics: is order correct here?
}
// Velocity change is just the linear movement along the contact normal
linearChange[i] = ContactNormal * linearMove[i];
// Apply the linear movement
Vector3 pos = Body[i]->GetPosition();
pos += linearMove[i] * ContactNormal;
Body[i]->SetPosition(pos);
// Apply the change in orientation
Quaternion rot = Body[i]->GetRotation();
rot.AddScaledVector(angularChange[i], 1.0);
Body[i]->SetRotation(rot);
// TODO calculate derived data for bodies that are not awake
}
}
}
