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 } } }