static RigTForm evaluateCatmull_Rom(RigTForm prev, RigTForm from, RigTForm to, RigTForm post, float alpha) {
// TODO: sanity check for quat negation if first coordinate of the part of D is negative
Cvec3 BC_CvecD = (to.getTranslation() - prev.getTranslation()) * (1.0 / 6.0) + from.getTranslation();
Cvec3 BC_CvecE = (post.getTranslation() - from.getTranslation()) * (-1.0 / 6.0) + to.getTranslation();
Quat q = prev.getRotation();
cout << "GETPREVROTATION: " << q[0] << q[1] << q[2] << q[3] << endl;
Quat d_pow = cn(to.getRotation() * inv(prev.getRotation()));
Quat e_pow = post.getRotation() * inv(from.getRotation());
cout << "=======\n";
cout << d_pow[0] << d_pow[1] << d_pow[2] << d_pow[3] << endl;
cout << e_pow[0] << e_pow[1] << e_pow[2] << e_pow[3] << endl;
cout << "------\n";
/*if (d_pow[0] < 0) {
d_pow = d_pow.cn;
printf("%i", d_pow[0]);
}*/
//Quat BC_QuatD = (to.getRotation() * inv(prev.getRotation())).quat_pow(1 / 6) * from.getRotation();
//Quat BC_QuatE = (post.getRotation() * inv(from.getRotation())).quat_pow(-1 / 6) * to.getRotation();
Quat BC_QuatD = d_pow.quat_pow(1.0 / 6.0) * from.getRotation();
Quat BC_QuatE = e_pow.quat_pow(-1.0 / 6.0) * to.getRotation();
// Quat BC_QuatD = Quat(1, 0, 0, 0);
// Quat BC_QuatE = Quat(1, 0, 0, 0);
RigTForm BC_D = RigTForm(BC_CvecD, BC_QuatD);
RigTForm BC_E = RigTForm(BC_CvecE, BC_QuatE);
return evaluateBezier(from, to, BC_D, BC_E, alpha);
}
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 writeFrameDataToFile() {
if (!keyframeList.empty()) {
ofstream myfile;
myfile.open("animation.txt");
// myfile << "Writing this to a file.\n";
// myfile << "testing, testing \n";
string output = "";
int num_frames = keyframeList.size();
list<RigTFormVector>::iterator iter = keyframeList.begin();
int num_nodes = (*iter).size();
std::ostringstream s;
s << num_frames
<< "\n"
<< num_nodes
<< "\n";
output.append(s.str());
for (iter; iter != keyframeList.end(); ++iter) {
RigTFormVector scene = (*iter);
for (int i = 0; i < scene.size(); ++i) {
RigTForm rigTForm = scene[i];
Cvec3 translation = rigTForm.getTranslation();
Quat rotation = rigTForm.getRotation();
for (int j = 0; j < 3; j++) {
s.str("");
s.clear();
s << translation[j] << " ";
output.append(s.str());
}
for (int k = 0; k < 4; k++) {
s.str("");
s.clear();
s << rotation[k] << " ";
output.append(s.str());
}
output.append("\n");
}
}
myfile << output;
myfile.close();
printf("Writing to animation.txt\n");
}
}
static void write_frame() {
list<vector<RigTForm> >::iterator it = key_frames.begin();
FILE* output = fopen("animation.txt", "w");
int n = (*it).size();
fprintf(output, "%d %d\n", key_frames.size(), n);
while (it != key_frames.end()) {
vector<RigTForm> frame = *it;
for (int i = 0; i < frame.size(); ++i) {
RigTForm r = frame[i];
Cvec3 transFact = r.getTranslation();
Quat linFact = r.getRotation();
fprintf(output, "%.3f %.3f %.3f %.3f %.3f %.3f %.3f\n",
transFact[0], transFact[1], transFact[2],
linFact[0], linFact[1], linFact[2], linFact[3]
);
}
++it;
}
fclose(output);
}
static RigTForm evaluateBezier(RigTForm from, RigTForm to, RigTForm d, RigTForm e, float alpha) {
Cvec3 f_t = lerp(from.getTranslation(), d.getTranslation(), alpha);
Cvec3 g_t = lerp(d.getTranslation(), e.getTranslation(), alpha);
Cvec3 h_t = lerp(e.getTranslation(), to.getTranslation(), alpha);
Cvec3 m_t = lerp(f_t, g_t, alpha);
Cvec3 n_t = lerp(g_t, h_t, alpha);
Cvec3 c_t = lerp(m_t, n_t, alpha);
Quat f_q = slerp(from.getRotation(), d.getRotation(), alpha);
Quat g_q = slerp(d.getRotation(), e.getRotation(), alpha);
Quat h_q = slerp(e.getRotation(), to.getRotation(), alpha);
Quat m_q = slerp(f_q, g_q, alpha);
Quat n_q = slerp(g_q, h_q, alpha);
Quat c_q = slerp(m_q, n_q, alpha);
return RigTForm(c_t, c_q);
}
static void motion(const int x, const int y) {
const double dx = x - g_mouseClickX;
const double dy = g_windowHeight - y - 1 - g_mouseClickY;
//initialize the outCenter and outRadius for the screenSpaceCircle
Cvec2 outCenter;
double outRadius;
//initialize the projection matrix for the screenSpaceCircle
const Matrix4 projmat = Matrix4::makeProjection(
g_frustFovY, g_windowWidth / static_cast <double> (g_windowHeight),
g_frustNear, g_frustFar);
if(currentView == 0)
eyeRbt = g_skyRbt;
else if (currentView == 1)
eyeRbt = g_objectRbt[0];
else
eyeRbt = g_objectRbt[1];
//gets the center for the screenSpaceCircle by passing in the center of the sphere in eye-coordinates
Cvec3 center = (inv(eyeRbt) * g_objectRbt[2]).getTranslation();
//getsthe screenSpaceCircle
getScreenSpaceCircle(center, 1, projmat, g_frustNear, g_frustFovY, g_windowWidth, g_windowHeight, outCenter, outRadius);
//get the two screen space vectors
Cvec2 p1((g_mouseClickX+dx)-outCenter(0), (dy + g_mouseClickY)-outCenter(1));
Cvec2 p2(g_mouseClickX-outCenter(0), g_mouseClickY-outCenter(1));
//clamp if we go outside the radius of the sphere
double dist1 = sqrt(pow(p1(0),2) + pow(p1(1),2));
if(dist1 > outRadius){
p1 = p1 * outRadius/(dist1+10); //+10 to avoid random rounding errors and stuff
}
double dist2 = sqrt(pow(p2(0),2) + pow(p2(1),2));
if (dist2 > outRadius){
p2 = p2 * outRadius/(dist2+10);
}
//Z-components for the projection
double currentZ = sqrt(pow(outRadius,2) - pow(p1(0),2) - pow(p1(1),2));
double transZ = sqrt(pow(outRadius,2) - pow(p2(0),2) - pow(p2(1),2));
//create two vectors for each mouse click with the tails at the origin of the sphere
Cvec3 currentV(p1, currentZ);
Cvec3 transV(p2, transZ);
//create two quaternions with normalized vectors
Quat qV1(0, normalize(currentV));
Quat qV2(0, normalize(transV));
//calculate the rotation quaternion
Quat Q = qV2 * inv(qV1);
RigTForm m;
if (g_mouseLClickButton && !g_mouseRClickButton){ // left button down?
m.setRotation(Q); //set the rotation quaternion
}
else if (g_mouseRClickButton && !g_mouseLClickButton) { // right button down?
if(currentObj==2 && currentAuxFrame==0)
m.setTranslation(Cvec3(dx, dy, 0) * -0.01);
else
m.setTranslation(Cvec3(dx, dy, 0) * 0.01);
}
else if (g_mouseMClickButton || (g_mouseLClickButton && g_mouseRClickButton)) { // middle or (left and right) button down?
m.setTranslation(Cvec3(0, 0, -dy) * 0.01);
}
RigTForm auxFrame; //initialize the auxillary frame
if (g_mouseClickDown) {
if(currentObj != 2){
m.setRotation(inv(m.getRotation()));
auxFrame.setTranslation(g_objectRbt[currentObj].getTranslation());
auxFrame.setRotation(eyeRbt.getRotation());
g_objectRbt[currentObj] = auxFrame * m * inv(auxFrame) * g_objectRbt[currentObj];//rotate around the object frame, translate around the sky frame
}
else if (currentObj == 2 && currentAuxFrame == 0){
auxFrame.setRotation(eyeRbt.getRotation());
g_skyRbt = auxFrame * m * inv(auxFrame) * g_skyRbt; //world-sky aux frame
}
else if (currentObj == 2 && currentAuxFrame == 1){
auxFrame.setTranslation(eyeRbt.getTranslation());
auxFrame.setRotation(eyeRbt.getRotation());
g_skyRbt = auxFrame * m * inv(auxFrame) * g_skyRbt; //sky-sky aux frame
}
glutPostRedisplay(); // we always redraw if we change the scene
}
g_mouseClickX = x;
g_mouseClickY = g_windowHeight - y - 1;
}
/*-----------------------------------------------*/
static void animateCamera(int value)
{
static float stopwatch = 0;
float msecsPerFrame = 1/(g_framesPerSecond / 1000);
static int animationPart = 0;
static bool isAnimating = true;
const static float stepsPerSecond = 10.0/2.0; // Time Allowed / Steps taken
static float totalTime = stepsPerSecond * 1 * 1000;
static float elapsedTime = 0;
static float x;
static float z;
static float radius = g_eyeRbt.getTranslation()[2];
static float helperDegrees = 0;
static float offsetDegrees = 90;
static bool isFirstEntry = true;
static RigTForm start = g_eyeRbt;
static RigTForm end = RigTForm(g_eyeRbt.getTranslation(), Quat::makeYRotation(-180) * start.getRotation());
//static RigTForm end = RigTForm();
// Used to reset variables every time animation is run
if (isFirstEntry)
{
start = g_eyeRbt;
radius = g_eyeRbt.getTranslation()[2];
end = RigTForm(g_eyeRbt.getTranslation(), Quat::makeYRotation(-180) * start.getRotation());
isFirstEntry = false;
//lookAtOrigin();
}
//Handles which part of animation is currently running
if (elapsedTime >= totalTime)
{
g_eyeRbt.setRotation(end.getRotation());
if (animationPart == 0)
{
start = end;
end = RigTForm(g_eyeRbt.getTranslation(), Quat::makeYRotation(-180) * start.getRotation());
helperDegrees = 180;
}
else
{
glutPostRedisplay();
isAnimating = false;
}
//Reset values to default
totalTime = stepsPerSecond * 1 * 1000;
elapsedTime = 0;
animationPart++;
}
if (isAnimating)
{
float alpha = elapsedTime / totalTime;
///*
//Handle Translation Interpolation
Cvec3 startVec = g_eyeRbt.getTranslation();
float degree = ((alpha * 180) + helperDegrees) + offsetDegrees;
float toRadians = CS175_PI / 180.0;
x = cos(degree * toRadians) * radius;
z = sin(degree * toRadians) * radius;
g_eyeRbt.setTranslation(Cvec3(x,g_eyeRbt.getTranslation()[1],z));
//*/
// Initial rotations
Quat startQ = start.getRotation();
// Final rotations
Quat endQ = end.getRotation();
// Handle Rotational Interpolation
if (g_interpolationType == I_POWER) // Quaternion Powering
{
if (endQ - startQ != Quat(0,0,0,0)) // Check for actual rotation
{
Quat currentQ = Quat::pow(endQ, alpha);
g_eyeRbt.setRotation(startQ * currentQ); // Apply rotation with respect to starting Position //Double rotates
}
}
else if (g_interpolationType == I_SLERP) //Spherical linear interpolation
{
//startQ = inv(Quat()) * startQ;// * Quat();
//endQ = inv(Quat()) * endQ;// * Quat();
g_eyeRbt.setRotation(Quat::slerp(startQ, endQ, alpha) * startQ);
}
else if (g_interpolationType == I_LERP)
{
//Normalize quaternions
Quat q = normalize(Quat::lerp(startQ, endQ, alpha));
g_eyeRbt.setRotation(q);
}
elapsedTime += msecsPerFrame;
glutPostRedisplay();
//Time total animation
stopwatch += msecsPerFrame;
glutTimerFunc(msecsPerFrame, animateCamera, 0);
}
else
{
isAnimating = true;
//cout << "Stopwatch Camera = " << (stopwatch - msecsPerFrame * 2) / 1000 << "\n"; // Display final time not counting first and last frame
stopwatch = 0;
animationPart = 0;
helperDegrees = 0;
isFirstEntry = true;
glutPostRedisplay();
}
}
/*-----------------------------------------------*/
static void animateLegs(int value)
{
static float stopwatch = 0;
float msecsPerFrame = 1/(g_framesPerSecond / 1000);
static int animationPart = 0;
static bool isAnimating = true;
const static float degreesPerStep = 30;
const static float stepsPerSecond = 20.0/34.0; // Time Allowed / Steps taken
RigTForm *leftLeg = &g_rigidBodies[0].children[0]->children[2]->children[2]->rtf;
RigTForm *rightLeg = &g_rigidBodies[0].children[0]->children[2]->children[3]->rtf;
static RigTForm startLeftLeg = *leftLeg;
static RigTForm endLeftLeg = startLeftLeg;
static RigTForm startRightLeg = *rightLeg;
static RigTForm endRightLeg = startRightLeg;
static float totalTime = stepsPerSecond * 1 * 1000;
static float elapsedTime = totalTime;
//Handles which part of animation is currently running
if (elapsedTime >= totalTime)
{
leftLeg->setRotation(endLeftLeg.getRotation());
rightLeg->setRotation(endRightLeg.getRotation());
// Initialize with first step
if (animationPart == 0)
{
startLeftLeg = endLeftLeg;
startRightLeg = endRightLeg;
endLeftLeg = RigTForm(Quat::makeXRotation(-degreesPerStep) * startLeftLeg.getRotation());
endRightLeg = RigTForm(Quat::makeXRotation(degreesPerStep) * startRightLeg.getRotation());
//cout << "endLeftLeg Angle = " << endLeftLeg.getRotation().getAngle() << "\n";
//cout << "endRightLeg Angle = " << endRightLeg.getRotation().getAngle() << "\n";
totalTime = stepsPerSecond * 0.5 * 1000;
}
else if (animationPart < 34)
{
startLeftLeg = endLeftLeg;
startRightLeg = endRightLeg;
if (animationPart %2 == 0)
{
endLeftLeg = RigTForm(Quat::makeXRotation(-degreesPerStep * 2) * startLeftLeg.getRotation());
endRightLeg = RigTForm(Quat::makeXRotation(degreesPerStep * 2) * startRightLeg.getRotation());
}
else
{
endLeftLeg = RigTForm(Quat::makeXRotation(degreesPerStep * 2) * startLeftLeg.getRotation());
endRightLeg = RigTForm(Quat::makeXRotation(-degreesPerStep * 2) * startRightLeg.getRotation());
}
//cout << "Degrees = " << degreesPerStep << "\n";
//cout << "endLeftLeg Angle = " << endLeftLeg.getRotation().getAngle() << "\n";
//cout << "endRightLeg Angle = " << endRightLeg.getRotation().getAngle() << "\n";
totalTime = stepsPerSecond * 1 * 1000;
}
else if (animationPart == 34)
{
startLeftLeg = endLeftLeg;
startRightLeg = endRightLeg;
endLeftLeg = RigTForm(Quat());
endRightLeg = RigTForm(Quat());
totalTime = stepsPerSecond * 0.5 * 1000;
}
else
{
glutPostRedisplay();
isAnimating = false;
//Reset values to default
startLeftLeg = *leftLeg;
startRightLeg = *rightLeg;
totalTime = stepsPerSecond * 1 * 1000;
}
animationPart++;
elapsedTime = 0;
}
if (isAnimating)
{
float alpha = elapsedTime / totalTime;
// Initial rotations
Quat startLeftLegQ = leftLeg->getRotation();
Quat startRightLegQ = rightLeg->getRotation();
// Final rotations
Quat endLeftLegQ = endLeftLeg.getRotation();
Quat endRightLegQ = endRightLeg.getRotation();
// Handle Rotational Interpolation
if (g_interpolationType == I_POWER) // Quaternion Powering
{
if (endLeftLegQ - startLeftLegQ != Quat(0,0,0,0)) // Check for actual rotation
{
Quat currentLeftLegQ = Quat::pow(endLeftLegQ, alpha);
leftLeg->setRotation(startLeftLegQ * currentLeftLegQ); // Apply rotation with respect to starting Position //Double rotates
}
if (endRightLegQ - startRightLegQ != Quat(0,0,0,0)) // Check for actual rotation
{
Quat currentRightLegQ = Quat::pow(endRightLegQ, alpha);
rightLeg->setRotation(startRightLegQ * currentRightLegQ); // Apply rotation with respect to starting Position //Double rotates
}
}
else if (g_interpolationType == I_SLERP) //Spherical linear interpolation
{
leftLeg->setRotation(Quat::slerp(startLeftLegQ, endLeftLegQ, alpha) * startLeftLegQ);
rightLeg->setRotation(Quat::slerp(startRightLegQ, endRightLegQ, alpha) * startRightLegQ);
}
else if (g_interpolationType == I_LERP)
{
//Normalize quaternions
Quat leftLegQ = normalize(Quat::lerp(startLeftLegQ, endLeftLegQ, alpha));
Quat rightLegQ = normalize(Quat::lerp(startRightLegQ, endRightLegQ, alpha));
leftLeg->setRotation(leftLegQ);
rightLeg->setRotation(rightLegQ);
}
elapsedTime += msecsPerFrame;
glutPostRedisplay();
//Time total animation
stopwatch += msecsPerFrame;
glutTimerFunc(msecsPerFrame, animateLegs, 0);
}
else
{
isAnimating = true;
//cout << "Stopwatch Legs = " << (stopwatch - msecsPerFrame * 2) / 1000 << "\n"; // Display final time not counting first and last frame
stopwatch = 0;
animationPart = 0;
elapsedTime = totalTime;
}
}
/*-----------------------------------------------*/
static void animateRobot(int value)
{
static float stopwatch = 0;
float msecsPerFrame = 1/(g_framesPerSecond / 1000);
static int animationPart = 0;
static bool isAnimating = true;
static float stepsPerSecond = 20.0/34.0; // Time Allowed / Steps taken
//Initial walk to right 5secs (+)x-axis
static RigTForm start = g_rigidBodies[0].rtf;
static RigTForm end = RigTForm(Cvec3(5,0,0)) * start;
static float totalTime = stepsPerSecond * 5 * 1000;
static float elapsedTime = 0;
//Handles which part of animation is currently running
if (elapsedTime > totalTime)
{
animationPart++;
//end = g_rigidBodies[0].rtf;
g_rigidBodies[0].rtf = end;
//Rotate
if (animationPart == 1 || animationPart == 3 || animationPart == 5 || animationPart == 7)
{
start = end;
end = RigTForm(start.getTranslation(), Quat::makeYRotation(90) * start.getRotation());
totalTime = stepsPerSecond * 1 * 1000;
}
//Walk (-)z 10 paces
else if (animationPart == 2)
{
start = end;
end = RigTForm(Cvec3(0,0,-10)) * start;
totalTime = stepsPerSecond * 10 * 1000;
}
//Walk (-)x 5 paces
else if (animationPart == 4)
{
start = end;
end = RigTForm(Cvec3(-5,0,0)) * start;
totalTime = stepsPerSecond * 5 * 1000;
}
//Walk (+)z 10 paces
else if (animationPart == 6)
{
start = end;
end = RigTForm(Cvec3(0,0,10)) * start;
totalTime = stepsPerSecond * 10 * 1000;
}
else
{
glutPostRedisplay();
isAnimating = false;
//Reset values to default
animationPart = 0;
start = g_rigidBodies[0].rtf;
end = RigTForm(Cvec3(5,0,0)) * start;
totalTime = stepsPerSecond * 5 * 1000;
}
elapsedTime = 0;
}
if (isAnimating)
{
float alpha = elapsedTime / totalTime;
//Handle Translation Interpolation
Cvec3 startVec = start.getTranslation();
Cvec3 temp = end.getTranslation() - startVec;
g_rigidBodies[0].rtf.setTranslation(startVec + (temp * alpha));
Quat startQ = start.getRotation(); // Initial rotation
Quat endQ = end.getRotation(); // Final rotation
//Handle Rotational Interpolation
if (g_interpolationType == I_POWER) // Quaternion Powering
{
if (endQ - startQ != Quat(0,0,0,0)) // Check for actual rotation
{
Quat currentQ = Quat::pow(endQ, alpha);
//Quat currentQ = Quat::pow(endQ * inv(startQ), alpha); // Calculate this frames rotation Quat //Slerping???
g_rigidBodies[0].rtf.setRotation(startQ * currentQ); // Apply rotation with respect to starting Position //Double rotates
}
}
else if (g_interpolationType == I_SLERP) //Spherical linear interpolation
{
g_rigidBodies[0].rtf.setRotation(Quat::slerp(startQ, endQ, alpha) * startQ);
}
else if (g_interpolationType == I_LERP)
{
Quat q = normalize(Quat::lerp(startQ, endQ, alpha)); //Normalize lerped quaternion
g_rigidBodies[0].rtf.setRotation(q);
}
elapsedTime += msecsPerFrame;
glutPostRedisplay();
//Time total animation
stopwatch += msecsPerFrame;
glutTimerFunc(msecsPerFrame, animateRobot, 0);
}
else
{
isAnimating = true;
//cout << "Stopwatch = " << (stopwatch - msecsPerFrame * 2) / 1000 << "\n"; // Display final time not counting first and last frame
stopwatch = 0;
}
}
/*-----------------------------------------------*/
static void keyboard(const unsigned char key, const int x, const int y)
{
/* PURPOSE: OpenGL Callback for Keyboard presses
RECEIVES: unsigned char key - Key pressed
int x - Mouse x_position when key pressed
int y - Mouse y_position when key pressed
REMARKS: Handles robot modifications based on key presses and then requests a redisplay
*/
if (isKeyboardActive)
{
switch (key)
{
case 27:
exit(0); // ESC
case 'i':
cout << " ============== H E L P ==============\n\n"
<< "i\t\thelp menu\n"
<< "s\t\tsave screenshot\n"
<< "f\t\tToggle flat shading on/off.\n"
<< "o\t\tCycle object to edit\n"
<< "v\t\tCycle view\n"
<< "drag left mouse to rotate\n" << endl;
break;
case 's':
glFlush();
writePpmScreenshot(g_windowWidth, g_windowHeight, "out.ppm");
break;
case 'f':
g_activeShader ^= 1;
break;
}
if (key == '1')
{
g_framesPerSecond = 32;
}
else if (key == '2')
{
g_framesPerSecond = 16;
}
else if (key == '3')
{
g_framesPerSecond = 8;
}
else if (key == '4')
{
g_framesPerSecond = 4;
}
else if (key == '5')
{
g_framesPerSecond = 2;
}
else if (key == '6')
{
g_framesPerSecond = 1;
}
else if (key == '7')
{
g_framesPerSecond = 0.5;
}
else if (key == '8')
{
g_framesPerSecond = 0.25;
}
else if (key == '9')
{
g_framesPerSecond = 0.125;
}
if (key == 'q')
{
Quat q = g_rigidBodies[0].rtf.getRotation();
g_rigidBodies[0].rtf.setRotation(q * Quat::makeYRotation(15));
}
else if (key == 'p')
{
g_interpolationType = I_POWER;
}
else if (key == 's')
{
g_interpolationType = I_SLERP;
}
else if (key == 'l')
{
g_interpolationType = I_LERP;
}
else if (key == 'r')
{
float msecs = 0 * 1000;
glutTimerFunc(msecs, timer, PAUSE);
glutTimerFunc(msecs, animateCamera,0);
}
else if (key == 'a')
{
float msecs = 0 * 1000;
glutTimerFunc(msecs, animateRobot, 0);
glutTimerFunc(msecs, animateLegs, 0);
}
else if (key == ',')
{
g_eyeRbt.setRotation(g_eyeRbt.getRotation() * Quat().makeZRotation(15));
}
else if (key == '.')
{
g_eyeRbt.setRotation(g_eyeRbt.getRotation() * Quat().makeZRotation(-15));
}
else if (key == '-')
{
float max = 20;
Cvec3 cameraTrans = g_eyeRbt.getTranslation();
g_eyeRbt.setTranslation(cameraTrans + Cvec3(0,0,1));
if (cameraTrans[2] >= max)
g_eyeRbt.setTranslation(Cvec3(cameraTrans[0], cameraTrans[1], max));
lookAtOrigin();
//cout << "( " << g_eyeRbt.getTranslation()[0] << ", " << g_eyeRbt.getTranslation()[1] << ", " << g_eyeRbt.getTranslation()[2] << "\n";
}
else if (key == '=')
{
float min = 5;
Cvec3 cameraTrans = g_eyeRbt.getTranslation();
g_eyeRbt.setTranslation(cameraTrans - Cvec3(0,0,1));
if (cameraTrans[2] <= min)
g_eyeRbt.setTranslation(Cvec3(cameraTrans[0], cameraTrans[1], min));
lookAtOrigin();
//cout << "( " << g_eyeRbt.getTranslation()[0] << ", " << g_eyeRbt.getTranslation()[1] << ", " << g_eyeRbt.getTranslation()[2] << "\n";
}
}
glutPostRedisplay();
}
//true on intersection, will populate passed Cvec3 pointer with intersection point
bool PhysicsSim::intersect_sphere_plane(shared_ptr<HasPhysicsBody> sphere, shared_ptr<HasPhysicsBody> plane, Cvec3* intersectionPoint) {
SphericalPhysicsBody *spherePb = dynamic_cast<SphericalPhysicsBody *>(sphere->getPhysicsBody());
shared_ptr<SgTransformNode> sphereNode = dynamic_pointer_cast<SgTransformNode>(sphere);
PlanarPhysicsBody *planePb = dynamic_cast<PlanarPhysicsBody *>(plane->getPhysicsBody());
shared_ptr<SgTransformNode> planeNode = dynamic_pointer_cast<SgTransformNode>(plane);
//assert sphere and plane!
assert(spherePb != NULL && sphereNode != NULL);
assert(planePb != NULL && planeNode != NULL);
Cvec3 sphereCenter = getPathAccumRbt(rootNode_, sphereNode).getTranslation();
RigTForm planeRtf = getPathAccumRbt(rootNode_, planeNode);
Cvec3 planeNormal = planeRtf.getRotation() * planePb->getUnitNormal();
//line is a line perp. to plane passing through sphere center
Cvec3 intersection = Cvec3();
bool doesIntersect =
intersect_line_plane(
sphereCenter,
planeNormal,
planeRtf.getTranslation(),
planeNormal,
&intersection
);
//constructed to be perp.
assert(doesIntersect);
Cvec3 perpVecFromCtrToPlane = intersection - sphereCenter;
float distFromCtrToPlane = norm(perpVecFromCtrToPlane);
//if the closest dist. from the center of the sphere to the plane
//is less than radius, then collision. Otherwise, no.
if(distFromCtrToPlane <= spherePb->getRadius()) {
//TODO: check that intersection lies within bounds of finite plane
//first: find quat from x-axis to the normal that has had the RigTForm's quat applied
//use this quat to transform the y and z axes
//get the intersection point relative to planar center
//apply the inverse quat from the first part
//check that abs(y) and ans(z) components are less than width/2 and height/2!!
Quat fromXAxisToNormal;
if(abs(dot(planeNormal, Cvec3(1, 0, 0))) - 1 < CS175_EPS2) {
//normal is parallel to x-axis
//Case 1: opposite
if(dot(planeNormal, Cvec3(1, 0, 0)) < 0) {
fromXAxisToNormal = Quat::makeZRotation(2 * M_PI);
} else {//Case 2:on the axis
fromXAxisToNormal = Quat::makeZRotation(0);
}
} else {
//then it's valid to do general formula
//from v1 to v2
Cvec3 a = cross(Cvec3(1, 0, 0), planeNormal);
fromXAxisToNormal[0] = a[0];
fromXAxisToNormal[1] = a[1];
fromXAxisToNormal[2] = a[2];
fromXAxisToNormal[3] = sqrt(norm2(planeNormal)) + planeNormal[0];
}
Quat fromNormalToXAxis = inv(fromXAxisToNormal);
Cvec3 intersectionOnXAxis = fromNormalToXAxis * (intersection - planeRtf.getTranslation());
if(abs(intersectionOnXAxis[1] <= planePb->getWidth()/2 && intersectionOnXAxis[2] <= planePb->getHeight()/2)) {
//fix sphere position to be firmly outside of plane!
float overlap = spherePb->getRadius() - distFromCtrToPlane;
Cvec3 offset = planeNormal * overlap;
sphereNode->setRbt(sphereNode->getRbt() * RigTForm(offset));
//intersection within bounds!
(*intersectionPoint) = intersection;
return true;
} else
return false;
} else
return false;
}
//called on mouse motion
static void motion(const int x, const int y) {
const double dx = x - g_mouseClickX;
const double dy = g_windowHeight - y - 1 - g_mouseClickY;
Matrix4 m;
RigTForm mRbt;
if (g_mouseLClickButton && !g_mouseRClickButton && !g_spaceDown) { // left button down?
if (g_arcballVisible){
mRbt = RigTForm(arcballRotation(x, y));
} else {
mRbt = RigTForm(Quat::makeXRotation(-dy) * Quat::makeYRotation(dx));
}
}
else if (g_mouseRClickButton && !g_mouseLClickButton) { // right button down?
if (g_arcballVisible) {
mRbt = RigTForm(Cvec3(dx, dy, 0) * g_arcballScale);
} else {
mRbt = RigTForm(Cvec3(dx, dy, 0) * 0.01);
}
}
else if (g_mouseMClickButton || (g_mouseLClickButton && g_mouseRClickButton) || (g_mouseLClickButton && !g_mouseRClickButton && g_spaceDown)) { // middle or (left and right, or left + space) button down?
if (g_arcballVisible) {
mRbt = RigTForm(Cvec3(0, 0, -dy) * g_arcballScale);
} else {
mRbt = RigTForm(Cvec3(0, 0, -dy) * 0.01);
}
}
buildAFrame();
if (g_mouseClickDown) {
RigTForm curRbt = g_currentPickedRbtNode->getRbt();
if(g_currentPickedRbtNode == g_skyNode) {
if (g_skyModMode == 0) {
Quat rot = mRbt.getRotation();
mRbt = RigTForm(-(mRbt.getTranslation()), Quat(rot[0], -(rot[1]), -(rot[2]), -(rot[3])));
g_skyNode->setRbt(g_aFrame * mRbt * inv(g_aFrame) * curRbt);
} else {
Quat rot = mRbt.getRotation();
mRbt = RigTForm(mRbt.getTranslation(), Quat(rot[0], -(rot[1]), -(rot[2]), -(rot[3])));
g_skyNode->setRbt(g_aFrame * mRbt * inv(g_aFrame) * curRbt);
}
}else {
//g_arcballRbt = g_aFrame * mRbt * inv(g_aFrame) * g_arcballRbt;
RigTForm newAFrame = inv(getPathAccumRbt(g_world, g_currentPickedRbtNode, 1)) * g_aFrame;
g_currentPickedRbtNode->setRbt(newAFrame * mRbt * inv(newAFrame) * curRbt);
}
glutPostRedisplay(); // we always redraw if we changed the scene
}
g_mouseClickX = x;
g_mouseClickY = g_windowHeight - y - 1;
}