void PhysicsSim::updateSphereSphereVelocities(int s1, int s2) {
shared_ptr<SgTransformNode> n1 = dynamic_pointer_cast<SgTransformNode>(sphericalBodies_[s1]);
shared_ptr<SgTransformNode> n2 = dynamic_pointer_cast<SgTransformNode>(sphericalBodies_[s2]);
Cvec3 pos1 = getPathAccumRbt(rootNode_, n1).getTranslation();
Cvec3 pos2 = getPathAccumRbt(rootNode_, n2).getTranslation();
SphericalPhysicsBody *sphere1 = dynamic_cast<SphericalPhysicsBody *>(sphericalBodies_[s1]->getPhysicsBody());
SphericalPhysicsBody *sphere2 = dynamic_cast<SphericalPhysicsBody *>(sphericalBodies_[s2]->getPhysicsBody());
Cvec3 u1 = sphere1->getVelocity();
Cvec3 u2 = sphere2->getVelocity();
double m1 = sphere1->getMass();
double m2 = sphere2->getMass();
Cvec3 pos = pos1 - pos2;
// if positions are too close, choose an arbitrary vector?
Cvec3 k = Cvec3(1,0,0);
if (dot(pos, pos) > ((1e-8) * (1e-8))) {
k = pos.normalize();
}
double a = (dot((k * 2.0), (u1 - u2)) * m1 * m2) / (m1 + m2);
Cvec3 v1 = u1 - (k * (a / m1));
Cvec3 v2 = u2 + (k * (a / m2));
sphere1->setVelocity(v1 * (1 - damping_));
sphere2->setVelocity(v2 * (1 - damping_));
}
void PhysicsSim::updateSpherePlaneVelocities(int s, int p) {
SphericalPhysicsBody *sphere = dynamic_cast<SphericalPhysicsBody *>(sphericalBodies_[s]->getPhysicsBody());
PlanarPhysicsBody *plane = dynamic_cast<PlanarPhysicsBody *>(planarBodies_[p]->getPhysicsBody());
shared_ptr<SgTransformNode> planeNode = dynamic_pointer_cast<SgTransformNode>(planarBodies_[p]);
RigTForm planeRtf = getPathAccumRbt(rootNode_, planeNode);
Cvec3 planeNormal = planeRtf.getRotation() * plane->getUnitNormal();
Cvec3 velocity = sphere->getVelocity();
planeNormal.normalize();
Cvec3 velDiff = -(planeNormal * (2 * dot(velocity, planeNormal)));
Cvec3 newVelocity = velocity + velDiff;
newVelocity *= (1 - damping_);
sphere->setVelocity(newVelocity);
}
static void updateMeshNormals(Mesh &mesh) {
for (int i = 0, n = mesh.getNumVertices(); i < n; i++) {
Cvec3 vectorSum = Cvec3();
Mesh::Vertex v = mesh.getVertex(i);
Mesh::VertexIterator vertexIter(v.getIterator()), iterOrigin(vertexIter);
// walk around the vertex
do {
vectorSum += vertexIter.getFace().getNormal();
} while (++vertexIter != iterOrigin);
if (dot(vectorSum, vectorSum) > CS175_EPS2) {
vectorSum.normalize();
}
v.setNormal(vectorSum);
}
}
static void motion(const int x, const int y) {
const double dx = x - g_mouseClickX;
const double dy = g_windowHeight - y - 1 - g_mouseClickY;
const double scale = (g_object == 0 && g_view == 0 && g_skyFrame == 1) ? .01 : g_arcballScale;
RigTForm m;
// Allow translation and rotation when the object is not the sky camera, or if it is, allow it ony if the eye frame is also the sky camera.
if (!(g_object == 0 && g_view != 0)) {
if (g_mouseLClickButton && !g_mouseRClickButton) { // left button down?
RigTForm mixedFrame;
if (g_object != g_view || (g_object == g_view && g_view == 0 && g_skyFrame == 0)) {
Cvec2 arcballCoord;
if (g_object == 0) {
arcballCoord = getScreenSpaceCoord((inv(g_viewRbt) * RigTForm(Cvec3(0, 0, 0))).getTranslation(), makeProjectionMatrix(), g_frustNear, g_frustFovY, g_windowWidth, g_windowHeight);
}
else {
arcballCoord = getScreenSpaceCoord((inv(g_viewRbt) * g_objRbt).getTranslation(), makeProjectionMatrix(), g_frustNear, g_frustFovY, g_windowWidth, g_windowHeight);
}
double xCoord = x - arcballCoord[0];
double yCoord = (g_windowHeight - y - 1) - arcballCoord[1];
// x**2 + y**2 + z**2 = r**2
double zSquared = g_arcballScreenRadius * g_arcballScreenRadius - xCoord * xCoord - yCoord * yCoord;
Cvec3 sphereCoord;
// Mouse is off of the arcball. Trackball movement.
if (zSquared < 0) {
//sphereCoord == v2
sphereCoord = Cvec3(xCoord, yCoord, 0);
}
else {
//sphereCoord == v3
sphereCoord = Cvec3(xCoord, yCoord, sqrt(zSquared));
}
sphereCoord.normalize();
if (g_v1[0] != 0 || g_v1[1] != 0 || g_v1[2] != 0) {
// [0 sphereCoord][0 -v1]
m = RigTForm(Quat(0, sphereCoord[0], sphereCoord[1], sphereCoord[2]) * Quat(0, -g_v1[0], -g_v1[1], -g_v1[2]));
}
if (g_object == 0 && zSquared >= 0) {
m = inv(m);
}
if (g_object == 0) {
mixedFrame = RigTForm(Cvec3(0, 0, 0)) * linFact(g_viewRbt);
// m = mixedFrame * m * inv(mixedFrame);
}
else {
mixedFrame = transFact(g_objRbt) * linFact(g_viewRbt);
// m = mixedFrame * m * inv(mixedFrame);
}
// g_objRbt = m * transFact(g_objRbt) * linFact(g_objRbt);
g_v1 = sphereCoord;
}
// Arcball is not used.
else {
m = RigTForm(Quat::makeXRotation(-dy) * Quat::makeYRotation(dx));
if (g_object == g_view) {
m = inv(m);
}
// Take M to O w.r.t mixedFrame
mixedFrame = transFact(g_objRbt) * linFact(g_viewRbt);
// m = mixedFrame * m * inv(mixedFrame);
// g_objRbt = m * transFact(g_objRbt) * linFact(g_objRbt);
}
m = mixedFrame * m * inv(mixedFrame);
g_objRbt = m * transFact(g_objRbt) * linFact(g_objRbt);
}
else if (g_mouseRClickButton && !g_mouseLClickButton) { // right button down?
m = RigTForm(Cvec3(dx,dy,0) * scale);
if (g_skyFrame == 0 && g_object == 0) {
m = inv(m);
}
// Take M to O w.r.t E
m = g_viewRbt * m * inv(g_viewRbt);
g_objRbt = transFact(g_objRbt) * m * linFact(g_objRbt);
}
else if (g_mouseMClickButton || (g_mouseLClickButton && g_mouseRClickButton)) { // middle or (left and right) button down?
m = RigTForm(Cvec3(0,0,-dy) * scale);
if (g_skyFrame == 0 && g_object == 0) {
m = inv(m);
}
// Take M to O w.r.t E
m = g_viewRbt * m * inv(g_viewRbt);
g_objRbt = transFact(g_objRbt) * m * linFact(g_objRbt);
}
}
if (g_mouseClickDown) {
if (g_object == 0) {
g_skyRbt = g_objRbt;
}
else if (g_object == 1) {
g_objectRbt[0] = g_objRbt;
}
else {
g_objectRbt[1] = g_objRbt;
}
glutPostRedisplay(); // we always redraw if we changed the scene
}
g_mouseClickX = x;
g_mouseClickY = g_windowHeight - y - 1;
}
