/*
* D3DParticleVelocityUpdate: Updates the velocity and position of a particle.
*/
void D3DParticleVelocityUpdate(emitter *pEmitter, particle *pParticle)
{
D3DMATRIX rotate, matrix;
custom_xyzw velocity;
if (pEmitter->emitterFlags & PS_GRAVITY)
pParticle->velocity.z -= 0.5f;
velocity.x = pParticle->velocity.x;
velocity.y = pParticle->velocity.y;
velocity.z = pParticle->velocity.z;
velocity.w = 1.0f;
MatrixRotateX(&matrix, pParticle->rotation.x);
MatrixRotateY(&rotate, pParticle->rotation.y);
MatrixMultiply(&rotate, &matrix, &rotate);
MatrixRotateZ(&matrix, pParticle->rotation.z);
MatrixMultiply(&rotate, &rotate, &matrix);
MatrixMultiplyVector(&velocity, &rotate, &velocity);
pParticle->velocity.x = velocity.x;
pParticle->velocity.y = velocity.y;
pParticle->velocity.z = velocity.z;
pParticle->oldPos.x = pParticle->pos.x;
pParticle->oldPos.y = pParticle->pos.y;
pParticle->oldPos.z = pParticle->pos.z;
pParticle->pos.x += pParticle->velocity.x;
pParticle->pos.y += pParticle->velocity.y;
pParticle->pos.z += pParticle->velocity.z;
}
void Util3D::TransRot(f32 xpos, f32 ypos, f32 rad)
{
Mtx FinalMatrix,TransMatrix;
MatrixRotateZ(TransMatrix,rad);
//guMtxRotRad(TransMatrix,'Z',rad); // Rotage
guMtxTransApply(TransMatrix,TransMatrix,xpos, ypos, 0.0f ); // Position
guMtxConcat(Singleton<WiiManager>::GetInstanceByRef().GetCamera()->GetcameraMatrix(),TransMatrix,FinalMatrix);
GX_LoadPosMtxImm (FinalMatrix, GX_PNMTX0);
}
void ArmGLWidget::drawArm(ArmShovelAngles A)
{
// Rover!
glColor3f (.5, .5, .5);
glTranslatef (-35, 5, 15);
drawBox(45, 15, 45);
glTranslatef (35, -5, -15);
glColor3f (1, 1, 1);
// Testing
matrix m = MatrixTranslate(0, 0, 0);
//m = MatrixRotateY(A.base.rotation) * m;
m = MatrixTranslate (-ArmController::BASE_OFFSET, 0, 0) * m;
m = MatrixRotateZ(A.base.shoulder) * m;
m = MatrixTranslate(ArmController::SEG1_LEN, 0, 0) * m;
//m = MatrixTranslate(0, 0, -OFFSET) * m;
m = MatrixRotateZ(A.base.elbow - 180) * m;
m = MatrixTranslate(40, 0, 0) * m;
glPushMatrix();
glMultMatrixf (*(m.array));
drawBox(1, 1, 1);
glPopMatrix();
glColor3f (1, 0, 0);
// Draw arm box
drawBox(15, 5, 15);
// Draw box to drop samples into
glPushMatrix();
// x = front to back, y = up and down, z = side to side
glTranslatef(0, - 2.5 + (4 + 5 - 2) * 2.54 , (23) * 2.54);
drawBox(7.25 * 2.54, 4 * 2.54, 6.5 * 2.54);
glPopMatrix();
glRotatef(A.base.rotation, 0, 1, 0);
glTranslatef(0, 3.5, 0);
drawCylinderY(3, 2);
glTranslatef(-ArmController::BASE_OFFSET, 2, 0);
drawCylinderZ(2, 6);
glColor3f (0, 1, 0);
glRotatef (A.base.shoulder, 0, 0, 1);//g.RotateTransform(-A.thetaS, System..Drawing2D.MatrixOrder.Prepend);
glTranslatef(ArmController::SEG1_LEN/2, 0, 0);
drawCylinderX(2, ArmController::SEG1_LEN);
glTranslatef(ArmController::SEG1_LEN/2, 0, -ArmController::OFFSET/2);
drawCylinderZ(2, ArmController::OFFSET + 4);
glTranslatef(0, 0, -ArmController::OFFSET/2);
glColor3f (1, 1, 0);
// Elbow to wrist elevation
glRotatef (A.base.elbow - 180, 0, 0, 1);//g.RotateTransform(180 - A.thetaE, System.Drawing.Drawing2D.MatrixOrder.Prepend);
float elbowToEnd = 35;
glTranslatef (elbowToEnd/2, 0, 0);
drawCylinderX(2, elbowToEnd);
glTranslatef (elbowToEnd/2, 0, 0);
drawCylinderZ(2, 4);
glColor3f (1, 0, 1);
glPushMatrix();
glRotatef(((float)A.head - 1000.0f) * 160.0f / 1024.0f, 0, 0, 1);
glPushMatrix();
glRotatef(((float)A.lid - 600.0f) * 300.0f / 1024.0f, 0, 0, 1);
this->drawBox(4, 1, 6);
glPopMatrix();
glRotatef(90, 0, 0, 1);
glTranslatef(2, 0, 0);
this->drawBox(4, 1, 8);
glTranslatef(2, 0, 0);
glRotatef(-90, 0, 0, 1);
glTranslatef(4, 0, 0);
this->drawBox(7, 1, 8);
glTranslatef(4, 0, 0);
glRotatef(-70, 0, 0, 1);
glTranslatef(2.5, 0, 0);
this->drawBox(5, 1, 8);
glPopMatrix();
/* float GRIPPER_ARM_LEN = 8;
static float r = 0;
r += 7;
//glRotatef (A.wristRotation, 1, 0, 0);
// CLAMPY CLAMPY!
static float a = 0;
a = sind(r) * 15;
drawCylinderX(4, 1);
glPushMatrix();
glTranslatef(0, 0, -4);
drawCylinderY(1, 2);
glRotatef(a, 0, 1, 0);
glTranslatef (GRIPPER_ARM_LEN/2, 0, 0);
drawCylinderX(1, GRIPPER_ARM_LEN);
glTranslatef (GRIPPER_ARM_LEN/2, 0, 0);
glRotatef(-a - 90, 0, 1, 0);
drawCylinderY(1, 2);
glTranslatef(1, 0, 0);
drawCylinderX(1, 2);
glPopMatrix();
glPushMatrix();
glTranslatef(0, 0, 4);
drawCylinderY(1, 2);
glRotatef(-a, 0, 1, 0);
glTranslatef (GRIPPER_ARM_LEN/2, 0, 0);
drawCylinderX(1, GRIPPER_ARM_LEN);
glTranslatef (GRIPPER_ARM_LEN/2, 0, 0);
glRotatef(a + 90, 0, 1, 0);
drawCylinderY(1, 2);
glTranslatef(1, 0, 0);
drawCylinderX(1, 2);
glPopMatrix();*/
}
void ArmGLWidget::drawArm(ArmAstroAngles A)
{
// Rover!
glColor3f (.5, .5, .5);
glTranslatef (-35, 5, 15);
drawBox(45, 15, 45);
glTranslatef (35, -5, -15);
glColor3f (1, 1, 1);
// Testing
matrix m = MatrixTranslate(0, 0, 0);
//m = MatrixRotateY(A.base.rotation) * m;
m = MatrixTranslate (-ArmController::BASE_OFFSET, 0, 0) * m;
m = MatrixRotateZ(A.base.shoulder) * m;
m = MatrixTranslate(ArmController::SEG1_LEN, 0, 0) * m;
//m = MatrixTranslate(0, 0, -OFFSET) * m;
m = MatrixRotateZ(A.base.elbow - 180) * m;
m = MatrixTranslate(40, 0, 0) * m;
glPushMatrix();
glMultMatrixf (*(m.array));
drawBox(1, 1, 1);
glPopMatrix();
glColor3f (1, 0, 0);
// Draw arm box
drawBox(15, 5, 15);
glRotatef(A.base.rotation, 0, 1, 0);
glTranslatef(0, 3.5, 0);
drawCylinderY(3, 2);
glTranslatef(-ArmController::BASE_OFFSET, 2, 0);
drawCylinderZ(2, 6);
glColor3f (0, 1, 0);
glRotatef (A.base.shoulder, 0, 0, 1);//g.RotateTransform(-A.thetaS, System..Drawing2D.MatrixOrder.Prepend);
glTranslatef(ArmController::SEG1_LEN/2, 0, 0);
drawCylinderX(2, ArmController::SEG1_LEN);
glTranslatef(ArmController::SEG1_LEN/2, 0, -ArmController::OFFSET/2);
drawCylinderZ(2, ArmController::OFFSET + 4);
glTranslatef(0, 0, -ArmController::OFFSET/2);
glColor3f (1, 1, 0);
// Elbow to wrist elevation
glRotatef (A.base.elbow - 180, 0, 0, 1);//g.RotateTransform(180 - A.thetaE, System.Drawing.Drawing2D.MatrixOrder.Prepend);
float elbowToEnd = 20;
glTranslatef (elbowToEnd/2, 0, 0);
drawCylinderX(2, elbowToEnd);
glTranslatef (elbowToEnd/2, 0, 0);
drawCylinderZ(2, 4);
//glColor3f (0, 0, 1);
// Wirst elevation to wrist rotate
//glRotatef(-A.wristElevation, 0, 0, 1);//g.RotateTransform(A.thetaW, System.Drawing.Drawing2D.MatrixOrder.Prepend);
//glTranslatef(ArmController::END_EFFECTOR_MID/2, 0, 0);
//drawCylinderX(2, ArmController::END_EFFECTOR_MID);
//glTranslatef(ArmController::END_EFFECTOR_MID/2, 0, 0);
//drawCylinderY(2, 4);
glColor3f (1, 0, 1);
float GRIPPER_ARM_LEN = 8;
//float HALF = ArmController::END_EFFECTOR_END - GRIPPER_ARM_LEN; // Point of rotation servo
//glRotatef(A.writsTip, 0, 1, 0);
//glTranslatef(HALF/2, 0, 0);
//drawCylinderX(2, HALF);
//glTranslatef(HALF/2, 0, 0);
//glColor3f (0, 1, 1);
static float r = 0;
r += 7;
//glRotatef (A.wristRotation, 1, 0, 0);
// CLAMPY CLAMPY!
static float a = 0;
a = sind(r) * 15;
drawCylinderX(4, 1);
glPushMatrix();
glTranslatef(0, 0, -4);
drawCylinderY(1, 2);
glRotatef(a, 0, 1, 0);
glTranslatef (GRIPPER_ARM_LEN/2, 0, 0);
drawCylinderX(1, GRIPPER_ARM_LEN);
glTranslatef (GRIPPER_ARM_LEN/2, 0, 0);
glRotatef(-a - 90, 0, 1, 0);
drawCylinderY(1, 2);
glTranslatef(1, 0, 0);
drawCylinderX(1, 2);
glPopMatrix();
glPushMatrix();
glTranslatef(0, 0, 4);
drawCylinderY(1, 2);
glRotatef(-a, 0, 1, 0);
glTranslatef (GRIPPER_ARM_LEN/2, 0, 0);
drawCylinderX(1, GRIPPER_ARM_LEN);
glTranslatef (GRIPPER_ARM_LEN/2, 0, 0);
glRotatef(a + 90, 0, 1, 0);
drawCylinderY(1, 2);
glTranslatef(1, 0, 0);
drawCylinderX(1, 2);
glPopMatrix();
}
