void ContactResolver::UpdateBodyPenetration( RigidBody* body, Contact& contact, int sign, const Vector3D<Real>& relativePos, const Vector3D<Real>& linearChange, const Vector3D<Real>& angularChange) { UNUSED(body) Vector3D<Real> deltaPosition = linearChange + angularChange.CrossProduct(relativePos); contact.m_penetration += deltaPosition.Dot(contact.m_contactNormal) * sign; }
void RigidBody::ApplyForceToWorldPoint(const Vector3D<Real>& worldPos, const Vector3D<Real>& force) { Vector3D<Real> position = worldPos; position -= GetPosition(); m_forceAccunulator += force; m_torqueAccumulator += position.CrossProduct(force); m_isAwake = true; }
void Camera::RotateZ (GLfloat Angle) { Vector3D up; Vector3D right; up = UpVector * cos(Angle) + RightVector* sin(Angle); right = -UpVector * sin(Angle) + RightVector* cos(Angle); ForwardVector = -right.CrossProduct(up); UpVector = up; RightVector = right; }
void Camera::RotateX (GLfloat Angle) { Vector3D fwd; Vector3D up; fwd = ForwardVector * cos(Angle) + UpVector* sin(Angle); up = -ForwardVector * sin(Angle) + UpVector* cos(Angle); up = fwd.CrossProduct(RightVector); ForwardVector = fwd; UpVector = -up; }
void Frustum::CalculateFrustum(float angle, float ratio, float near, float far, Vector3D &camPos, Vector3D &lookAt, Vector3D &up) { Vector3D xVec, yVec, zVec; Vector3D vecN, vecF; Vector3D nearTopLeft, nearTopRight, nearBottomLeft, nearBottomRight; Vector3D farTopLeft, farTopRight, farBottomLeft, farBottomRight; float radians = (float)tan((DEG_TO_RAD(angle)) * 0.5); float nearH = near * radians; float nearW = nearH * ratio; float farH = far * radians; float farW = farH * ratio; zVec = camPos - lookAt; zVec.Normalize(); xVec = up.CrossProduct(zVec); xVec.Normalize(); yVec = zVec.CrossProduct(xVec); vecN = camPos - zVec * near; vecF = camPos - zVec * far; nearTopLeft = vecN + yVec * nearH - xVec * nearW; nearTopRight = vecN + yVec * nearH + xVec * nearW; nearBottomLeft = vecN - yVec * nearH - xVec * nearW; nearBottomRight = vecN - yVec * nearH + xVec * nearW; farTopLeft = vecF + yVec * farH - xVec * farW; farTopRight = vecF + yVec * farH + xVec * farW; farBottomLeft = vecF - yVec * farH - xVec * farW; farBottomRight = vecF - yVec * farH + xVec * farW; m_frustum.clear(); Plane plane; plane.CreatePlaneFromTri(nearTopRight, nearTopLeft, farTopLeft); AddPlane(plane); plane.CreatePlaneFromTri(nearBottomLeft, nearBottomRight, farBottomRight); AddPlane(plane); plane.CreatePlaneFromTri(nearTopLeft, nearBottomLeft, farBottomLeft); AddPlane(plane); plane.CreatePlaneFromTri(nearBottomRight, nearTopRight, farBottomRight); AddPlane(plane); plane.CreatePlaneFromTri(nearTopLeft, nearTopRight, nearBottomRight); AddPlane(plane); plane.CreatePlaneFromTri(farTopRight, farTopLeft, farBottomLeft); AddPlane(plane); }