/* Just interpolate linearly yaw, pitch, and roll. Doing this _correctly_
* (I.E get the same results as quat_interpolate) would require one to use
* linear integration, a subject that is in the last 100 pages of my 1500
* page Calculus book. This function is an example of what you should NOT
* do, as in some cases it will cause the orientation to swing wildly about.
* The path could be anything from nearly correct, a spiral, or a curly Q.
* The simple solution is to use quaternion interpolation, which always
* results in a simple circular path.
*/
static void euler_interpolate(ref EULER from, ref EULER to, float t, ref EULER _out)
{
float delta;
delta = (to.x - from.x) * t;
_out.x = from.x + delta;
delta = (to.y - from.y) * t;
_out.y = from.y + delta;
delta = (to.z - from.z) * t;
_out.z = from.z + delta;
}
private void Metodo_Euler(bool guia)
{
//Inicializo variables con parámetros de entrada
double a = Convert.ToDouble(tbA.Text);
double b = Convert.ToDouble(tbB.Text);
double x0 = Convert.ToDouble(tbX0.Text);
int n = Convert.ToInt32(tbN.Text);
string fx = tbFx.Text;
double k = Convert.ToDouble(tbT.Text ?? "0");
//Instancio objeto euler
EULER euler = new EULER(a, b, x0, n, fx, k);
//Calculo tabla de puntos
euler.Calcular();
//chResultado.Series.Clear();
//Creo serie para los valores calculados
var serieEuler = new Series
{
Name = "EULER",
Color = System.Drawing.Color.Green,
IsVisibleInLegend = true,
IsXValueIndexed = false,
ChartType = SeriesChartType.Line,
};
//Agrego serie al gráfico
this.chResultado.Series.Add(serieEuler);
//Seteo intervalo del eje t con el h calculado
this.chResultado.ChartAreas[0].AxisX.Interval = euler.h;
this.chResultado.ChartAreas[0].AxisY.IntervalAutoMode = IntervalAutoMode.VariableCount;
this.chResultado.ChartAreas[0].RecalculateAxesScale();
var i = 0;
//Recorro resultados y agrego valores a la serie y muestro en consola
foreach (var p in euler.resultado)
{
serieEuler.Points.AddXY(p.x, p.fx);
Console.Write(String.Format("t{0}: {1} ", i, p.x));
Console.Write(String.Format("x({0}): {1}\n", i, p.fx));
i++;
}
Console.WriteLine("===============================");
chResultado.Invalidate();
//Si se requiere una guia de la función, uso el mismo metodo pero con n=100000
if (guia)
{
var euler_m = new EULER_MEJORADO(a, b, x0, 10000, fx, k);
euler_m.Calcular();
var series2 = new Series
{
Name = "FUNCION",
Color = System.Drawing.Color.Red,
IsVisibleInLegend = true,
IsXValueIndexed = false,
ChartType = SeriesChartType.Line
};
this.chResultado.Series.Add(series2);
foreach (var p in euler_m.resultado)
{
series2.Points.AddXY(p.x, p.fx);
}
}
}
static int Main()
{
int index;
if (allegro_init() != 0)
{
return(1);
}
install_keyboard();
if (set_gfx_mode(GFX_AUTODETECT, 640, 480, 0, 0) != 0)
{
if (set_gfx_mode(GFX_SAFE, 640, 480, 0, 0) != 0)
{
set_gfx_mode(GFX_TEXT, 0, 0, 0, 0);
allegro_message(string.Format("Unable to set any graphic mode\n{0}\n", allegro_error));
return(1);
}
}
set_palette(desktop_palette);
clear_to_color(screen, palette_color[0]);
/* Each back-buffer is one quarter the size of the screen
*/
euler_buffer = create_bitmap(320, 240);
quat_buffer = create_bitmap(320, 240);
if ((!euler_buffer) || (!quat_buffer))
{
set_gfx_mode(GFX_TEXT, 0, 0, 0, 0);
allegro_message("Error creating bitmaps\n");
return(1);
}
set_palette(desktop_palette);
/* setup the viewport for rendering into the back-buffers */
set_projection_viewport(0, 0, 320, 240);
/* print out something helpful for the user */
textout_ex(screen, font, "SPACE - next interpolation", 184, 24, palette_color[15], -1);
textout_ex(screen, font, " R - repeat last interpolation", 184, 40, palette_color[15], -1);
textout_ex(screen, font, " ESC - quit", 184, 56, palette_color[15], -1);
textout_ex(screen, font, "Interpolating Euler Angles", 56, 110, palette_color[15], -1);
textout_ex(screen, font, "Interpolating Quaternions", 380, 110, palette_color[15], -1);
textout_ex(screen, font, "Incorrect!", 120, 360, palette_color[15], -1);
textout_ex(screen, font, "Correct!", 448, 360, palette_color[15], -1);
/* initialize the path edges. This structure is used by both the Euler
* path and the quaternion path. It connects all the points end to end
*/
for (index = 0; index < (NUM_STEPS - 1); index++)
{
path_edges[index, 0] = index;
path_edges[index, 1] = index + 1;
}
/* initialize the first destination orientation */
//srand(time(NULL));
e_to.x = (float)(AL_RAND() % 256);
e_to.y = (float)(AL_RAND() % 256);
e_to.z = (float)(AL_RAND() % 256);
/* the camera is backed away from the origin and turned to face it */
get_camera_matrix_f(ref camera, 5, 0, 0, -1, 0, 0, 0, 0, 1, 46, 1.33f);
/* this is a for-ever loop */
for (; ;)
{
float t;
clear_keybuf();
for (index = 0; index < (NUM_STEPS + 1); index++)
{
if (keypressed())
{
break;
}
t = (float)(index * (1.0 / NUM_STEPS));
/* the first part shows how to animate the cube incorrectly
* using Euler angles
*/
/* create the matrix for the starting orientation */
get_rotation_matrix_f(ref rotation, e_from.x, e_from.y, e_from.z);
matrix_mul_f(ref rotation, ref camera, out e_from_matrix);
/* create the matrix for the ending orientation */
get_rotation_matrix_f(ref rotation, e_to.x, e_to.y, e_to.z);
matrix_mul_f(ref rotation, ref camera, out e_to_matrix);
/* use the incorrect method to interpolate between them */
euler_interpolate(ref e_from, ref e_to, t, ref e_in);
get_rotation_matrix_f(ref rotation, e_in.x, e_in.y, e_in.z);
matrix_mul_f(ref rotation, ref camera, out e_in_matrix);
/* update the lines that make up the Euler orientation path */
apply_matrix_f(ref rotation, 0, 0, 1.5F,
ref (e_path_points_1[index, 0]),
ref (e_path_points_1[index, 1]),
ref (e_path_points_1[index, 2]));
apply_matrix_f(ref rotation, 0, 0, 2.0F,
ref (e_path_points_2[index, 0]),
ref (e_path_points_2[index, 1]),
ref (e_path_points_2[index, 2]));
/* render the results to the Euler sub-bitmap */
clear_to_color(euler_buffer, palette_color[0]);
render_demo_box(euler_buffer, ref e_from_matrix, ref e_in_matrix, ref e_to_matrix,
palette_color[15], palette_color[1], palette_color[4]);
render_wireframe_object(ref camera, euler_buffer, e_path_points_1,
tmp_points, path_edges, index + 1, index,
palette_color[5]);
render_wireframe_object(ref camera, euler_buffer, e_path_points_2,
tmp_points, path_edges, index + 1, index,
palette_color[5]);
/* here is how to animate the cube correctly using quaternions */
/* create a matrix for the starting orientation. This time
* we create it using quaternions. This is to demonstrate
* that the quaternion gotten with get_rotation_quat will
* generate the same matrix as that gotten by get_rotation_matrix
*/
get_rotation_quat(ref q_from, e_from.x, e_from.y, e_from.z);
quat_to_matrix(ref q_from, ref rotation);
matrix_mul_f(ref rotation, ref camera, out q_from_matrix);
/* this is the same as above, but for the ending orientation */
get_rotation_quat(ref q_to, e_to.x, e_to.y, e_to.z);
quat_to_matrix(ref q_to, ref rotation);
matrix_mul_f(ref rotation, ref camera, out q_to_matrix);
/* quat_interpolate is the proper way to interpolate between two
* orientations.
*/
quat_interpolate(ref q_from, ref q_to, t, out q_in);
quat_to_matrix(ref q_in, ref rotation);
matrix_mul_f(ref rotation, ref camera, out q_in_matrix);
/* update the lines that make up the quaternion orientation path */
apply_matrix_f(ref rotation, 0, 0, 1.5F,
ref (q_path_points_1[index, 0]),
ref (q_path_points_1[index, 1]),
ref (q_path_points_1[index, 2]));
apply_matrix_f(ref rotation, 0, 0, 2.0F,
ref (q_path_points_2[index, 0]),
ref (q_path_points_2[index, 1]),
ref (q_path_points_2[index, 2]));
/* render the results to the quaternion sub-bitmap */
clear_to_color(quat_buffer, palette_color[0]);
render_demo_box(quat_buffer, ref q_from_matrix, ref q_in_matrix, ref q_to_matrix,
palette_color[15], palette_color[1], palette_color[4]);
render_wireframe_object(ref camera, quat_buffer, q_path_points_1,
tmp_points, path_edges, index + 1, index,
palette_color[5]);
render_wireframe_object(ref camera, quat_buffer, q_path_points_2,
tmp_points, path_edges, index + 1, index,
palette_color[5]);
acquire_bitmap(screen);
blit(euler_buffer, screen, 0, 0, 0, 120, 320, 240);
blit(quat_buffer, screen, 0, 0, 320, 120, 320, 240);
release_bitmap(screen);
rest(1);
}
/* handle user input */
for (; ;)
{
int input = readkey() >> 8;
if (input == KEY_R)
{
/* skip updating the EULER angles so that the last interpolation
* will repeat
*/
break;
}
else if (input == KEY_SPACE)
{
/* make the last ending orientation the starting orientation and
* generate a random new ending orientation
*/
e_from = e_to;
e_to.x = (float)(AL_RAND() % 256);
e_to.y = (float)(AL_RAND() % 256);
e_to.z = (float)(AL_RAND() % 256);
break;
}
else if (input == KEY_ESC)
{
/* quit the program */
destroy_bitmap(euler_buffer);
destroy_bitmap(quat_buffer);
return(0);
}
}
}
}
