const char*
transform_scene_node(const char *node, matrixgl_t mat, matrixgl_t invmat )
{
scene_node_t *nodePtr;
if ( get_scene_node( node, &nodePtr ) != TCL_OK ) {
return "No such node";
}
multiply_matrices( nodePtr->trans, nodePtr->trans, mat );
multiply_matrices( nodePtr->invtrans, invmat, nodePtr->invtrans );
return NULL;
}
/*---------------------------------------------------------------------+
| main |
| ==================================================================== |
| |
| Function: ... |
| |
+---------------------------------------------------------------------*/
int main (int argc, char **argv)
{
long start_time; /* time at start of testcase */
int i;
/*
* Setup signal handler & setup alarm so we do not loop forever...
*/
signal (SIGUSR1, signal_handler);
signal (SIGALRM, signal_handler);
alarm (600); /* wait 10 minutes before aborting */
/*
* Process command line arguments...
*/
parse_args (argc, argv);
if (verbose) printf ("%s: Scheduler TestSuite program\n\n", *argv);
if (debug) {
printf ("\tpriority: %s\n", priority);
}
/*
* Adjust the priority of this process if the real time flag is set
*/
if (!strcmp (priority, "fixed")) {
#ifndef __linux__
if (setpri (0, DEFAULT_PRIORITY) < 0)
sys_error ("setpri failed", __FILE__, __LINE__);
#else
if (setpriority(PRIO_PROCESS, 0, 0) < 0)
sys_error ("setpri failed", __FILE__, __LINE__);
#endif
}
/*
* Continuously multiply matrix as time permits...
*/
i = 0;
start_time = time ((long *) 0);
/*
* Continuously read through file until interrupted...
*/
if (debug) printf ("\n");
while (!signaled) {
if (debug) {
printf ("\r\tmultiplying matrix [%d]", i++);
fflush (stdout);
}
multiply_matrices ();
}
if (debug) printf ("\n");
/*
* Exit with success!
*/
if (verbose) printf ("\nsuccessful!\n");
return (0);
}
const char*
rotate_scene_node( const char *node, char axis, scalar_t angle )
{
scene_node_t *nodePtr;
matrixgl_t rotMatrix;
if ( get_scene_node( node, &nodePtr ) != TCL_OK ) {
return "No such node";
}
make_rotation_matrix( rotMatrix, angle, axis );
multiply_matrices( nodePtr->trans, nodePtr->trans, rotMatrix );
make_rotation_matrix( rotMatrix, -angle, axis );
multiply_matrices( nodePtr->invtrans, rotMatrix, nodePtr->invtrans );
return NULL;
}
const char*
translate_scene_node(const char *node, vector_t vec )
{
scene_node_t *nodePtr;
matrixgl_t xlateMatrix;
if ( get_scene_node( node, &nodePtr ) != TCL_OK ) {
return "No such node";
}
make_translation_matrix( xlateMatrix, vec.x, vec.y, vec.z );
multiply_matrices( nodePtr->trans, nodePtr->trans, xlateMatrix );
make_translation_matrix( xlateMatrix, -vec.x, -vec.y, -vec.z );
multiply_matrices( nodePtr->invtrans, xlateMatrix, nodePtr->invtrans );
return NULL;
}
/*---------------------------------------------------------------------+
| main |
| ==================================================================== |
| |
| Function: ... |
| |
+---------------------------------------------------------------------*/
int main (int argc, char **argv)
{
long start_time; /* time at start of testcase */
int i;
/*
* Process command line arguments...
*/
if (argc < 2) {
fprintf (stderr, USAGE, *argv);
exit (0);
}
parse_args (argc, argv);
if (verbose) printf ("%s: Scheduler TestSuite program\n\n", *argv);
if (debug) {
printf ("\tpriority: %s\n", priority);
printf ("\texecution_time: %ld (sec)\n", execution_time);
}
/*
* Adjust the priority of this process if the real time flag is set
*/
if (!strcmp (priority, "fixed")) {
#ifndef __linux__
if (setpri (0, DEFAULT_PRIORITY) < 0)
sys_error ("setpri failed", __FILE__, __LINE__);
#else
if (setpriority(PRIO_PROCESS, 0, 0) < 0)
sys_error ("setpri failed", __FILE__, __LINE__);
#endif
}
/*
* Continuously multiply matrix as time permits...
*/
i = 0;
start_time = time ((long *) 0);
if (debug) printf ("\n");
while ( (time ((long *)0) - start_time) < execution_time) {
if (debug) {
printf ("\r\tmultiplying matrix [%d], time left: %ld",
i++,
execution_time - (time ((long *)0)-start_time));
fflush (stdout);
}
multiply_matrices ();
}
if (debug) printf ("\n");
/*
* Exit with success!
*/
if (verbose) printf ("\nsuccessful!\n");
return (0);
}
int main(int argc, char** argv)
{
int matrix_size;
int silent = 0;
int optchar;
elem_t *a, *inv, *prod;
elem_t eps;
double error;
double ratio;
while ((optchar = getopt(argc, argv, "qt:")) != EOF)
{
switch (optchar)
{
case 'q': silent = 1; break;
case 't': s_nthread = atoi(optarg); break;
default:
fprintf(stderr, "Error: unknown option '%c'.\n", optchar);
return 1;
}
}
if (optind + 1 != argc)
{
fprintf(stderr, "Error: wrong number of arguments.\n");
}
matrix_size = atoi(argv[optind]);
/* Error checking. */
assert(matrix_size >= 1);
assert(s_nthread >= 1);
eps = epsilon();
a = new_matrix(matrix_size, matrix_size);
init_matrix(a, matrix_size, matrix_size);
inv = invert_matrix(a, matrix_size);
prod = multiply_matrices(a, matrix_size, matrix_size,
inv, matrix_size, matrix_size);
error = identity_error(prod, matrix_size);
ratio = error / (eps * matrix_size);
if (! silent)
{
printf("error = %g; epsilon = %g; error / (epsilon * n) = %g\n",
error, eps, ratio);
}
if (isfinite(ratio) && ratio < 100)
printf("Error within bounds.\n");
else
printf("Error out of bounds.\n");
delete_matrix(prod);
delete_matrix(inv);
delete_matrix(a);
return 0;
}
const char*
scale_scene_node(const char *node, point_t center, scalar_t factor[3] )
{
scene_node_t *nodePtr;
matrixgl_t matrix;
if ( get_scene_node( node, &nodePtr ) != TCL_OK ) {
return "No such node";
}
make_translation_matrix( matrix, -center.x, -center.y, -center.z );
multiply_matrices( nodePtr->trans, nodePtr->trans, matrix );
make_translation_matrix( matrix, center.x, center.y, center.z );
multiply_matrices( nodePtr->invtrans, matrix, nodePtr->invtrans );
make_scaling_matrix( matrix, factor[0], factor[1], factor[2] );
multiply_matrices( nodePtr->trans, nodePtr->trans, matrix );
make_scaling_matrix( matrix, 1./factor[0], 1./factor[1], 1./factor[2] );
multiply_matrices( nodePtr->invtrans, matrix, nodePtr->invtrans );
make_translation_matrix( matrix, center.x, center.y, center.z );
multiply_matrices( nodePtr->trans, nodePtr->trans, matrix );
make_translation_matrix( matrix, -center.x, -center.y, -center.z );
multiply_matrices( nodePtr->invtrans, matrix, nodePtr->invtrans );
return NULL;
}
bool_t check_polyhedron_collision_with_dag(
scene_node_t *node, matrixgl_t modelMatrix, matrixgl_t invModelMatrix,
polyhedron_t ph)
{
matrixgl_t newModelMatrix, newInvModelMatrix;
scene_node_t *child;
polyhedron_t newph;
bool_t hit = False;
check_assertion( node != NULL, "node is NULL" );
multiply_matrices( newModelMatrix, modelMatrix, node->trans );
multiply_matrices( newInvModelMatrix, node->invtrans, invModelMatrix );
if ( node->geom == Sphere ) {
newph = copy_polyhedron( ph );
trans_polyhedron( newInvModelMatrix, newph );
hit = intersect_polyhedron( newph );
free_polyhedron( newph );
}
if (hit == True) return hit;
child = node->child;
while (child != NULL) {
hit = check_polyhedron_collision_with_dag(
child, newModelMatrix, newInvModelMatrix, ph );
if ( hit == True ) {
return hit;
}
child = child->next;
}
return False;
}
static int reset(NiggliParams *p)
{
double *lat_tmp;
lat_tmp = NULL;
if ((lat_tmp = multiply_matrices(p->lattice, p->tmat)) == NULL) {return 0;}
memcpy(p->lattice, lat_tmp, sizeof(double) * 9);
free(lat_tmp);
lat_tmp = NULL;
return set_parameters(p);
}
static double * get_metric(const double *M)
{
double *G, *M_T;
G = NULL;
M_T = NULL;
if ((M_T = get_transpose(M)) == NULL) {return NULL;}
if ((G = multiply_matrices(M_T, M)) == NULL) {return NULL;}
free(M_T);
M_T = NULL;
return G;
}
void test_multiply_matrices() {
int matrix_size = 3;
double matrix1[matrix_size][matrix_size];
double matrix2[matrix_size][matrix_size];
double result[matrix_size][matrix_size];
int i, j;
for (i = 0; i < matrix_size; i++) {
for (j = 0; j < matrix_size; j++){
matrix1[i][j] = (i+1.0);
matrix2[i][j] = (i +1.0);
result[i][j] = 0.0;
}
}
multiply_matrices(&result[0][0], &matrix1[0][0], &matrix2[0][0], matrix_size ) ;
print_matrices(&result[0][0], &matrix1[0][0], &matrix2[0][0], matrix_size ) ;
}
/*******************************************************************************
* matrix_t invert_matrix(matrix_t A)
*
* Invert Matrix function based on LUP decomposition and then forward and
* backward substitution.
*******************************************************************************/
matrix_t invert_matrix(matrix_t A){
int i,j,k,m;
matrix_t L,U,P,D,temp;
matrix_t out = create_empty_matrix();
if(!A.initialized){
printf("ERROR: matrix not initialized yet\n");
return out;
}
if(A.cols != A.rows){
printf("ERROR: matrix is not square\n");
return out;
}
if(matrix_determinant(A) == 0){
printf("ERROR: matrix is singular, not invertible\n");
return out;
}
m = A.cols;
LUP_decomposition(A,&L,&U,&P);
D = create_identity_matrix(m);
temp = create_square_matrix(m);
for(j=0;j<m;j++){
for(i=0;i<m;i++){
for(k=0;k<i;k++){
D.data[i][j] -= L.data[i][k] * D.data[k][j];
}
}
for(i=m-1;i>=0;i--){ // backwards.. last to first
temp.data[i][j] = D.data[i][j];
for(k=i+1;k<m;k++){
temp.data[i][j] -= U.data[i][k] * temp.data[k][j];
}
temp.data[i][j] = temp.data[i][j] / U.data[i][i];
}
}
// multiply by permutation matrix
out = multiply_matrices(temp, P);
// free allocation
destroy_matrix(&temp);
destroy_matrix(&L);
destroy_matrix(&U);
destroy_matrix(&P);
destroy_matrix(&D);
return out;
}
void traverse_dag_for_view_point( scene_node_t *node, matrixgl_t trans )
{
matrixgl_t new_trans;
scene_node_t *child;
check_assertion( node != NULL, "node is NULL" );
multiply_matrices( new_trans, trans, node->trans );
if ( node->eye == True ) {
set_tux_eye( node->which_eye,
transform_point( new_trans, make_point( 0., 0., 0. ) ) );
}
child = node->child;
while (child != NULL) {
traverse_dag_for_view_point( child, new_trans );
child = child->next;
}
}
int main(int argc, char** argv)
{
int matrix_size;
int silent;
elem_t *a, *inv, *prod;
elem_t eps;
double error;
double ratio;
matrix_size = (argc > 1) ? atoi(argv[1]) : 3;
s_nthread = (argc > 2) ? atoi(argv[2]) : 3;
silent = (argc > 3) ? atoi(argv[3]) : 0;
eps = epsilon();
a = new_matrix(matrix_size, matrix_size);
init_matrix(a, matrix_size, matrix_size);
inv = invert_matrix(a, matrix_size);
prod = multiply_matrices(a, matrix_size, matrix_size,
inv, matrix_size, matrix_size);
error = identity_error(prod, matrix_size);
ratio = error / (eps * matrix_size);
if (! silent)
{
printf("error = %g; epsilon = %g; error / (epsilon * n) = %g\n",
error, eps, ratio);
}
if (ratio < 100)
printf("Error within bounds.\n");
else
printf("Error out of bounds.\n");
delete_matrix(prod);
delete_matrix(inv);
delete_matrix(a);
return 0;
}
/*******************************************************************************
* int QR_decomposition(matrix_t A, matrix_t* Q, matrix_t* R)
*
*
*******************************************************************************/
int QR_decomposition(matrix_t A, matrix_t* Q, matrix_t* R){
int i, j, k, s;
int m = A.rows;
int n = A.cols;
vector_t xtemp;
matrix_t Qt, Rt, Qi, F, temp;
if(!A.initialized){
printf("ERROR: matrix not initialized yet\n");
return -1;
}
destroy_matrix(Q);
destroy_matrix(R);
Qt = create_matrix(m,m);
for(i=0;i<m;i++){ // initialize Qt as I
Qt.data[i][i] = 1;
}
Rt = duplicate_matrix(A); // duplicate A to Rt
for(i=0;i<n;i++){ // iterate through columns of A
xtemp = create_vector(m-i); // allocate length, decreases with i
for(j=i;j<m;j++){ // take col of -R from diag down
xtemp.data[j-i] = -Rt.data[j][i];
}
if(Rt.data[i][i] > 0) s = -1; // check the sign
else s = 1;
xtemp.data[0] += s*vector_norm(xtemp); // add norm to 1st element
Qi = create_square_matrix(m); // initialize Qi
F = create_square_matrix(m-i); // initialize shrinking householder_matrix
F = householder_matrix(xtemp); // fill in Househodor
for(j=0;j<i;j++){
Qi.data[j][j] = 1; // fill in partial I matrix
}
for(j=i;j<m;j++){ // fill in remainder (householder_matrix)
for(k=i;k<m;k++){
Qi.data[j][k] = F.data[j-i][k-i];
}
}
// multiply new Qi to old Qtemp
temp = duplicate_matrix(Qt);
destroy_matrix(&Qt);
Qt = multiply_matrices(Qi,temp);
destroy_matrix(&temp);
// same with Rtemp
temp = duplicate_matrix(Rt);
destroy_matrix(&Rt);
Rt = multiply_matrices(Qi,temp);
destroy_matrix(&temp);
// free other allocation used in this step
destroy_matrix(&Qi);
destroy_matrix(&F);
destroy_vector(&xtemp);
}
transpose_matrix(&Qt);
*Q = Qt;
*R = Rt;
return 0;
}
void update_key_frame( player_data_t *plyr, scalar_t dt )
{
int idx;
scalar_t frac;
point_t pos;
scalar_t v;
matrixgl_t cob_mat, rot_mat;
char *root;
char *lsh;
char *rsh;
char *lhp;
char *rhp;
char *lkn;
char *rkn;
char *lank;
char *rank;
char *head;
char *neck;
char *tail;
root = get_tux_root_node();
lsh = get_tux_left_shoulder_joint();
rsh = get_tux_right_shoulder_joint();
lhp = get_tux_left_hip_joint();
rhp = get_tux_right_hip_joint();
lkn = get_tux_left_knee_joint();
rkn = get_tux_right_knee_joint();
lank = get_tux_left_ankle_joint();
rank = get_tux_right_ankle_joint();
head = get_tux_head();
neck = get_tux_neck();
tail = get_tux_tail_joint();
keyTime += dt;
for (idx = 1; idx < numFrames; idx ++) {
if ( keyTime < frames[idx].time )
break;
}
if ( idx == numFrames || numFrames == 0 ) {
set_game_mode( RACING );
return;
}
reset_scene_node( root );
reset_scene_node( lsh );
reset_scene_node( rsh );
reset_scene_node( lhp );
reset_scene_node( rhp );
reset_scene_node( lkn );
reset_scene_node( rkn );
reset_scene_node( lank );
reset_scene_node( rank );
reset_scene_node( head );
reset_scene_node( neck );
reset_scene_node( tail );
check_assertion( idx > 0, "invalid keyframe index" );
if ( fabs( frames[idx-1].time - frames[idx].time ) < EPS ) {
frac = 1.;
} else {
frac = (keyTime - frames[idx].time)
/ ( frames[idx-1].time - frames[idx].time );
}
pos.x = interp( frac, frames[idx-1].pos.x, frames[idx].pos.x );
pos.z = interp( frac, frames[idx-1].pos.z, frames[idx].pos.z );
pos.y = interp( frac, frames[idx-1].pos.y, frames[idx].pos.y );
pos.y += find_y_coord( pos.x, pos.z );
set_tux_pos( plyr, pos );
make_identity_matrix( cob_mat );
v = interp( frac, frames[idx-1].yaw, frames[idx].yaw );
rotate_scene_node( root, 'y', v );
make_rotation_matrix( rot_mat, v, 'y' );
multiply_matrices( cob_mat, cob_mat, rot_mat );
v = interp( frac, frames[idx-1].pitch, frames[idx].pitch );
rotate_scene_node( root, 'x', v );
make_rotation_matrix( rot_mat, v, 'x' );
multiply_matrices( cob_mat, cob_mat, rot_mat );
v = interp( frac, frames[idx-1].l_shldr, frames[idx].l_shldr );
rotate_scene_node( lsh, 'z', v );
v = interp( frac, frames[idx-1].r_shldr, frames[idx].r_shldr );
rotate_scene_node( rsh, 'z', v );
v = interp( frac, frames[idx-1].l_hip, frames[idx].l_hip );
rotate_scene_node( lhp, 'z', v );
v = interp( frac, frames[idx-1].r_hip, frames[idx].r_hip );
rotate_scene_node( rhp, 'z', v );
/* Set orientation */
plyr->orientation = make_quaternion_from_matrix( cob_mat );
plyr->orientation_initialized = True;
}
int main(void)
{
int retval = 0;
int choice = 0;
short *prandom_data;
#ifdef PATCHED_1
char m_result_data[MAX_ROWS * MAX_COLS * sizeof(int)];
#else
char m_result_data[((MAX_ROWS * MAX_COLS) - 1) * sizeof(int)];
#endif
prandom_data = create_random_shorts();
matrix_t *m;
matrix_t *m1, *m2;
matrix_t *m_result;
m1 = create_matrix(SHORT, NULL);
m2 = create_matrix(SHORT, NULL);
m_result = create_matrix(INT, m_result_data);
char *input = malloc(2048);
printf("Matrix math is fun!\n");
printf("-------------------\n");
while (1)
{
choice = select_menu_choice(input, LINE_SIZE);
switch(choice)
{
case 1:
printf("Inputting Matrix Values:\n");
m = choose_matrix(m1, m2, input, LINE_SIZE);
if (!m)
goto cgc_exit;
if (input_matrix(m, input, LINE_SIZE) == ERROR)
goto cgc_exit;
break;
case 2:
printf("Print Matrices:\n");
print_matrices(m1, m2, m_result);
break;
case 3:
printf("Adding Matrices:\n");
add_matrices(m1, m2, m_result);
break;
case 4:
printf("Subtracting Matrices:\n");
subtract_matrices(m1, m2, m_result);
break;
case 5:
printf("Multiplying Matrices:\n");
multiply_matrices(m1, m2, m_result);
break;
case 6:
printf("Swap Rows in a Matrix:\n");
m = choose_matrix(m1, m2, input, LINE_SIZE);
if (!m)
goto cgc_exit;
retval = swap_matrix_row_col(m, SWAP_ROW, input, LINE_SIZE);
if (retval == ERROR)
goto cgc_exit;
if (retval == SUCCESS)
print_matrix("Swapped Rows", m);
break;
case 7:
printf("Swap Columns in a Matrix:\n");
m = choose_matrix(m1, m2, input, LINE_SIZE);
if (!m)
goto cgc_exit;
retval = swap_matrix_row_col(m, SWAP_COL, input, LINE_SIZE);
if (retval == ERROR)
goto cgc_exit;
if (retval == SUCCESS)
print_matrix("Swapped Columns", m);
break;
case 8:
printf("Transpose a Matrix:\n");
m = choose_matrix(m1, m2, input, LINE_SIZE);
if (!m)
goto cgc_exit;
transpose_matrix(m);
break;
case 9:
printf("Perform Reduced Row Echelon Form on Matrix\n");
m = choose_matrix(m1, m2, input, LINE_SIZE);
if (!m)
goto cgc_exit;
rref_matrix(m, m_result);
break;
case 10:
printf("Create a Random Matrix:\n");
m = choose_matrix(m1, m2, input, LINE_SIZE);
if (!m)
goto cgc_exit;
if (random_matrix(m, input, LINE_SIZE, prandom_data) == ERROR)
goto cgc_exit;
break;
case 11:
goto cgc_exit;
default:
printf("Bad Selection\n");
}
}
cgc_exit:
printf("Exiting...\n");
return 0;
}
int fixedwing_control_main(int argc, char *argv[])
{
/* default values for arguments */
char *fixedwing_uart_name = "/dev/ttyS1";
char *commandline_usage = "\tusage: %s -d fixedwing-devicename\n";
/* read arguments */
bool verbose = false;
for (int i = 1; i < argc; i++) {
if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) {
if (argc > i + 1) {
fixedwing_uart_name = argv[i + 1];
} else {
printf(commandline_usage, argv[0]);
return 0;
}
}
if (strcmp(argv[i], "-v") == 0 || strcmp(argv[i], "--verbose") == 0) {
verbose = true;
}
}
/* welcome user */
printf("[fixedwing control] started\n");
/* Set up to publish fixed wing control messages */
struct fixedwing_control_s control;
int fixedwing_control_pub = orb_advertise(ORB_ID(fixedwing_control), &control);
/* Subscribe to global position, attitude and rc */
struct vehicle_global_position_s global_pos;
int global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
struct vehicle_attitude_s att;
int attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
struct rc_channels_s rc;
int rc_sub = orb_subscribe(ORB_ID(rc_channels));
struct vehicle_global_position_setpoint_s global_setpoint;
int global_setpoint_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
/* Mainloop setup */
unsigned int loopcounter = 0;
unsigned int failcounter = 0;
/* Control constants */
control_outputs.mode = HIL_MODE;
control_outputs.nav_mode = 0;
/* Servo setup */
int fd;
servo_position_t data[PWM_OUTPUT_MAX_CHANNELS];
fd = open("/dev/pwm_servo", O_RDWR);
if (fd < 0) {
printf("[fixedwing control] Failed opening /dev/pwm_servo\n");
}
ioctl(fd, PWM_SERVO_ARM, 0);
int16_t buffer_rc[3];
int16_t buffer_servo[3];
mixer_data_t mixer_buffer;
mixer_buffer.input = buffer_rc;
mixer_buffer.output = buffer_servo;
mixer_conf_t mixers[3];
mixers[0].source = PITCH;
mixers[0].nr_actuators = 2;
mixers[0].dest[0] = AIL_1;
mixers[0].dest[1] = AIL_2;
mixers[0].dual_rate[0] = 1;
mixers[0].dual_rate[1] = 1;
mixers[1].source = ROLL;
mixers[1].nr_actuators = 2;
mixers[1].dest[0] = AIL_1;
mixers[1].dest[1] = AIL_2;
mixers[1].dual_rate[0] = 1;
mixers[1].dual_rate[1] = -1;
mixers[2].source = THROTTLE;
mixers[2].nr_actuators = 1;
mixers[2].dest[0] = MOT;
mixers[2].dual_rate[0] = 1;
/*
* Main control, navigation and servo routine
*/
while(1) {
/*
* DATA Handling
* Fetch current flight data
*/
/* get position, attitude and rc inputs */
// XXX add error checking
orb_copy(ORB_ID(vehicle_global_position), global_pos_sub, &global_pos);
orb_copy(ORB_ID(vehicle_global_position_setpoint), global_setpoint_sub, &global_setpoint);
orb_copy(ORB_ID(vehicle_attitude), attitude_sub, &att);
orb_copy(ORB_ID(rc_channels), rc_sub, &rc);
/* scaling factors are defined by the data from the APM Planner
* TODO: ifdef for other parameters (HIL/Real world switch)
*/
/* position values*/
plane_data.lat = global_pos.lat; /// 10000000.0;
plane_data.lon = global_pos.lon; /// 10000000.0;
plane_data.alt = global_pos.alt; /// 1000.0f;
plane_data.vx = global_pos.vx / 100.0f;
plane_data.vy = global_pos.vy / 100.0f;
plane_data.vz = global_pos.vz / 100.0f;
/* attitude values*/
plane_data.roll = att.roll;
plane_data.pitch = att.pitch;
plane_data.yaw = att.yaw;
plane_data.rollspeed = att.rollspeed;
plane_data.pitchspeed = att.pitchspeed;
plane_data.yawspeed = att.yawspeed;
/* parameter values */
get_parameters(&plane_data, &pid_s);
/* Attitude control part */
if (verbose && loopcounter % 20 == 0) {
/******************************** DEBUG OUTPUT ************************************************************/
printf("Parameter: %i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i,%i \n", (int)pid_s.Kp_att, (int)pid_s.Ki_att,
(int)pid_s.Kd_att, (int)pid_s.intmax_att, (int)pid_s.Kp_pos, (int)pid_s.Ki_pos,
(int)pid_s.Kd_pos, (int)pid_s.intmax_pos, (int)plane_data.airspeed,
(int)plane_data.wp_x, (int)plane_data.wp_y, (int)plane_data.wp_z,
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLON],
(int)global_data_parameter_storage->pm.param_values[PARAM_WPLAT],
(int)global_data_parameter_storage->pm.param_values[PARAM_WPALT],
global_setpoint.lat,
global_setpoint.lon,
(int)global_setpoint.altitude);
printf("PITCH SETPOINT: %i\n", (int)(100.0f*plane_data.pitch_setpoint));
printf("ROLL SETPOINT: %i\n", (int)(100.0f*plane_data.roll_setpoint));
printf("THROTTLE SETPOINT: %i\n", (int)(100.0f*plane_data.throttle_setpoint));
printf("\n\nVx: %i\t Vy: %i\t Current speed:%i\n\n", (int)plane_data.vx, (int)plane_data.vy, (int)(calc_gnd_speed(&plane_data)));
printf("Current Position: \n %i \n %i \n %i \n", (int)plane_data.lat, (int)plane_data.lon, (int)plane_data.alt);
printf("\nAttitude values: \n R:%i \n P: %i \n Y: %i \n\n RS: %i \n PS: %i \n YS: %i \n ",
(int)(180.0f * plane_data.roll/F_M_PI), (int)(180.0f * plane_data.pitch/F_M_PI), (int)(180.0f * plane_data.yaw/F_M_PI),
(int)(180.0f * plane_data.rollspeed/F_M_PI), (int)(180.0f * plane_data.pitchspeed/F_M_PI), (int)(180.0f * plane_data.yawspeed)/F_M_PI);
printf("\nCalculated outputs: \n R: %i\n P: %i\n Y: %i\n T: %i \n",
(int)(10000.0f * control_outputs.roll_ailerons), (int)(10000.0f * control_outputs.pitch_elevator),
(int)(10000.0f * control_outputs.yaw_rudder), (int)(10000.0f * control_outputs.throttle));
/************************************************************************************************************/
}
/*
* Computation section
*
* The function calls to compute the required control values happen
* in this section.
*/
/* Set plane mode */
set_plane_mode(&plane_data);
/* Calculate the output values */
control_outputs.roll_ailerons = calc_roll_ail(&plane_data, &pid_s);
control_outputs.pitch_elevator = calc_pitch_elev(&plane_data, &pid_s);
control_outputs.yaw_rudder = calc_yaw_rudder(&plane_data, &pid_s);
control_outputs.throttle = calc_throttle(&plane_data, &pid_s);
/*
* Calculate rotation matrices
*/
calc_rotation_matrix(&rmatrix_att, plane_data.roll, plane_data.pitch, plane_data.yaw);
calc_rotation_matrix(&rmatrix_c, control_outputs.roll_ailerons, control_outputs.pitch_elevator, control_outputs.yaw_rudder);
multiply_matrices(&rmatrix_att, &rmatrix_c, &rmatrix_b);
calc_angles_from_rotation_matrix(&rmatrix_b, plane_data.rollb, plane_data.pitchb, plane_data.yawb);
// TODO: fix RC input
//if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // if we're flying in automated mode
/*
* TAKEOFF hack for HIL
*/
if (plane_data.mode == TAKEOFF) {
control.attitude_control_output[ROLL] = 0;
control.attitude_control_output[PITCH] = 5000;
control.attitude_control_output[THROTTLE] = 10000;
//global_data_fixedwing_control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
if (plane_data.mode == CRUISE) {
// TODO: substitute output with the body angles (rollb, pitchb)
control.attitude_control_output[ROLL] = control_outputs.roll_ailerons;
control.attitude_control_output[PITCH] = control_outputs.pitch_elevator;
control.attitude_control_output[THROTTLE] = control_outputs.throttle;
//control->attitude_control_output[YAW] = (int16_t)(control_outputs.yaw_rudder);
}
control.counter++;
control.timestamp = hrt_absolute_time();
//}
/* Navigation part */
/*
* TODO: APM Planner Waypoint communication
*/
// Get GPS Waypoint
// plane_data.wp_x = global_setpoint.lon;
// plane_data.wp_y = global_setpoint.lat;
// plane_data.wp_z = global_setpoint.altitude;
/*
* TODO: fix RC input
*/
//if (rc.chan[rc.function[OVERRIDE]].scale < MANUAL) { // AUTO mode
// AUTO/HYBRID switch
//if (rc.chan[rc.function[OVERRIDE]].scale < AUTO) {
plane_data.roll_setpoint = calc_roll_setpoint(35.0f, &plane_data);
plane_data.pitch_setpoint = calc_pitch_setpoint(35.0f, &plane_data);
plane_data.throttle_setpoint = calc_throttle_setpoint(&plane_data);
// } else {
// plane_data.roll_setpoint = rc.chan[rc.function[ROLL]].scale / 200;
// plane_data.pitch_setpoint = rc.chan[rc.function[PITCH]].scale / 200;
// plane_data.throttle_setpoint = rc.chan[rc.function[THROTTLE]].scale / 200;
// }
//control_outputs.yaw_rudder = calc_yaw_rudder(plane_data.hdg);
// } else {
// control.attitude_control_output[ROLL] = rc.chan[rc.function[ROLL]].scale/10000;
// control.attitude_control_output[PITCH] = rc.chan[rc.function[PITCH]].scale/10000;
// control.attitude_control_output[THROTTLE] = rc.chan[rc.function[THROTTLE]].scale/10000;
// since we don't have a yaw rudder
//control.attitude_control_output[YAW] = rc.chan[rc.function[YAW]].scale/10000;
control.counter++;
control.timestamp = hrt_absolute_time();
//}
/* publish the control data */
orb_publish(ORB_ID(fixedwing_control), fixedwing_control_pub, &control);
/* Servo part */
buffer_rc[ROLL] = (int16_t)(10000*control.attitude_control_output[ROLL]);
buffer_rc[PITCH] = (int16_t)(10000*control.attitude_control_output[PITCH]);
buffer_rc[THROTTLE] = (int16_t)(10000*control.attitude_control_output[THROTTLE]);
//mix_and_link(mixers, 3, 2, &mixer_buffer);
// Scaling and saturation of servo outputs happens here
data[AIL_1] = buffer_servo[AIL_1] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO1_TRIM];
if (data[AIL_1] > SERVO_MAX)
data[AIL_1] = SERVO_MAX;
if (data[AIL_1] < SERVO_MIN)
data[AIL_1] = SERVO_MIN;
data[AIL_2] = buffer_servo[AIL_2] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO2_TRIM];
if (data[AIL_2] > SERVO_MAX)
data[AIL_2] = SERVO_MAX;
if (data[AIL_2] < SERVO_MIN)
data[AIL_2] = SERVO_MIN;
data[MOT] = buffer_servo[MOT] / global_data_parameter_storage->pm.param_values[PARAM_SERVO_SCALE]
+ global_data_parameter_storage->pm.param_values[PARAM_SERVO3_TRIM];
if (data[MOT] > SERVO_MAX)
data[MOT] = SERVO_MAX;
if (data[MOT] < SERVO_MIN)
data[MOT] = SERVO_MIN;
int result = write(fd, &data, sizeof(data));
if (result != sizeof(data)) {
if (failcounter < 10 || failcounter % 20 == 0) {
printf("[fixedwing_control] failed writing servo outputs\n");
}
failcounter++;
}
loopcounter++;
/* 20Hz loop*/
usleep(50000);
}
return 0;
}
int main(void) {
clock_t begin, end;
double time_spent;
begin = clock();
matrix* a = create_matrix(4, 4);
value temp_a[16] = { 18, 60, 57, 96,
41, 24, 99, 58,
14, 30, 97, 66,
51, 13, 19, 85 };
insert_array(temp_a, a);
matrix* b = create_matrix(4, 4);
assert(insert_array(temp_a, b));
//tests check_boundaries
assert(check_boundaries(1,1,a));
assert(check_boundaries(4,4,a));
assert(!check_boundaries(4,5,a));
assert(!check_boundaries(5,4,a));
assert(!check_boundaries(0,1,a));
assert(!check_boundaries(1,0,a));
assert(!check_boundaries(-1,1,a));
assert(!check_boundaries(1,-1,a));
//tests compare_matrices,insert_value and get_value
assert(compare_matrices(a,b));
assert(insert_value(10,1,1,b));
assert(!compare_matrices(a,b));
assert(get_value(1,1,b)==10);
assert(insert_value(18,1,1,b));
assert(compare_matrices(a,b));
//tests is_matrix
matrix* c=a;
assert(compare_matrices(a,c));
assert(!is_matrix(a,b));
assert(is_matrix(a,c));
//tests insert_value by trying to go outside the matrix
assert(insert_value(1,1,1,c));
assert(insert_value(2,2,2,c));
assert(insert_value(3,3,3,c));
assert(insert_value(4,4,4,c));
assert(!insert_value(5,5,5,c));
assert(!insert_value(-1,-1,-1,c));
assert(!insert_value(-1,-1,1,c));
assert(!insert_value(-1,1,-1,c));
//test get_value
assert(get_value(1,1,c)==1);
assert(get_value(2,2,c)==2);
assert(get_value(3,3,c)==3);
assert(get_value(4,4,c)==4);
assert(get_value(0,0,c)==0);
assert(get_value(1,-1,c)==0);
assert(get_value(-1,1,c)==0);
assert(get_value(5,5,c)==0);
//tests insert and get without boundary checks
insert_value_without_check(4,1,1,c);
insert_value_without_check(3,2,2,c);
insert_value_without_check(2,3,3,c);
insert_value_without_check(1,4,4,c);
assert(get_value_without_check(1,1,c)==4);
assert(get_value_without_check(2,2,c)==3);
assert(get_value_without_check(3,3,c)==2);
assert(get_value_without_check(4,4,c)==1);
//tests add_matrices
value temp_b[16]={
36,120,114,192,
82,48,198,116,
28, 60, 194,132,
102,26,38,170};
assert(insert_array(temp_b,a));
matrix* d = create_matrix(4, 4);
assert(add_matrices(b,b,d));
assert(compare_matrices(d,a));
//tests subtract_matrices
value temp_c[16]={
0,0,0,0,
0,0,0,0,
0, 0, 0,0,
0,0,0,0};
assert(insert_array(temp_c,a));
assert(subtract_matrices(b,b,d));
assert(compare_matrices(d,a));
//tests sum_of_row
assert(insert_array(temp_a,a));
assert(sum_of_row(1,a)==231);
assert(sum_of_row(4,a)==168);
assert(sum_of_row(0,a)==0);
assert(sum_of_row(5,a)==0);
//tests sum_of_column
assert(sum_of_column(1,a)==124);
assert(sum_of_column(4,a)==305);
assert(sum_of_column(0,a)==0);
assert(sum_of_column(5,a)==0);
//tests get_row_vector
matrix* e = create_matrix(1, 4);
value temp_d[4] = { 18, 60, 57, 96};
assert(insert_array(temp_d,e));
matrix* f = create_matrix(1, 4);
assert(!get_row_vector(0,a,f));
assert(!get_row_vector(5,a,f));
assert(get_row_vector(1,a,f));
assert(compare_matrices(e,f));
//tests get_column_vector
matrix* g = create_matrix(4, 1);
assert(insert_array(temp_d,e));
matrix* h = create_matrix(1, 4);
assert(!get_row_vector(0,a,h));
assert(!get_row_vector(5,a,h));
assert(get_row_vector(1,a,h));
assert(compare_matrices(e,h));
//tests mulitply_matrices
assert(multiply_matrices(a,a,b));
value temp_f[16]={8478,5478,14319,17130,
6066,6760,15418,16792,
6206,5328,14431,15096,
6052,5047,7652,14129.00};
assert(insert_array(temp_f,d));
assert(compare_matrices(b,d));
assert(!multiply_matrices(a,h,b));
assert(!multiply_matrices(a,a,h));
//tests transpose_matrix
value temp_g[16]={18,41,14,51,
60,24,30,13,
57,99,97,19,
96,58,66,85};
assert(insert_array(temp_g,d));
assert(transpose_matrix(a,b));
assert(compare_matrices(b,d));
assert(!transpose_matrix(e,b));
assert(!transpose_matrix(a,e));
//tests multiply_matrix_with_scalar
value temp_h[16] = { 36, 120, 114, 192,
82, 48, 198, 116,
28, 60, 194, 132,
102, 26, 38, 170 };
assert(insert_array(temp_h,b));
multiply_matrix_with_scalar(2,a);
assert(compare_matrices(a,b));
//test get_sub_matrix
matrix* i=create_matrix(2,2);
assert(insert_array(temp_a,a));
assert(get_sub_matrix(1,2,1,2,a,i));
matrix* j=create_matrix(2,2);
value temp_i[4] = { 18, 60, 41, 24};
assert(insert_array(temp_i,j));
assert(compare_matrices(j,i));
value temp_j[4] = { 97, 66, 19, 85};
assert(insert_array(temp_j,j));
assert(get_sub_matrix(3,4,3,4,a,i));
assert(compare_matrices(j,i));
assert(!get_sub_matrix(2,4,3,4,a,i));
assert(!get_sub_matrix(3,4,2,4,a,i));
assert(!get_sub_matrix(4,5,4,5,a,i));
assert(!get_sub_matrix(0,1,0,1,a,i));
//test insert_row_vector
assert(insert_array(temp_a,a));
value temp_k[16] = { 18, 60, 57, 96,
18, 60, 57, 96,
14, 30, 97, 66,
51, 13, 19, 85 };
assert(insert_array(temp_k,b));
assert(insert_array(temp_d,e));
assert(insert_row_vector(2,e,a));
assert(compare_matrices(a,b));
end = clock();
time_spent = (double)(end - begin) / CLOCKS_PER_SEC;
printf("time taken was: %f \n",time_spent);
free_matrix(a);
free_matrix(b);
free_matrix(d);
free_matrix(e);
free_matrix(f);
free_matrix(g);
free_matrix(h);
free_matrix(i);
free_matrix(j);
return 0;
}
int main(){
printf("Let's test some linear algebra functions....\n\n");
// create a random nxn matrix for later use
matrix_t A = create_random_matrix(DIM,DIM);
printf("New Random Matrix A:\n");
print_matrix(A);
// also create random vector
vector_t b = create_random_vector(DIM);
printf("\nNew Random Vector b:\n");
print_vector(b);
// do an LUP decomposition on A
matrix_t L,U,P;
LUP_decomposition(A,&L,&U,&P);
printf("\nL:\n");
print_matrix(L);
printf("U:\n");
print_matrix(U);
printf("P:\n");
print_matrix(P);
// do a QR decomposition on A
matrix_t Q,R;
QR_decomposition(A,&Q,&R);
printf("\nQR Decomposition of A\n");
printf("Q:\n");
print_matrix(Q);
printf("R:\n");
print_matrix(R);
// get determinant of A
float det = matrix_determinant(A);
printf("\nDeterminant of A : %8.4f\n", det);
// get an inverse for A
matrix_t Ainv = invert_matrix(A);
if(A.initialized != 1) return -1;
printf("\nAinverse\n");
print_matrix(Ainv);
// multiply A times A inverse
matrix_t AA = multiply_matrices(A,Ainv);
if(AA.initialized!=1) return -1;
printf("\nA * Ainverse:\n");
print_matrix(AA);
// solve a square linear system
vector_t x = lin_system_solve(A, b);
printf("\nGaussian Elimination solution x to the equation Ax=b:\n");
print_vector(x);
// now do again but with qr decomposition method
vector_t xqr = lin_system_solve_qr(A, b);
printf("\nQR solution x to the equation Ax=b:\n");
print_vector(xqr);
// If b are the coefficients of a polynomial, get the coefficients of the
// new polynomial b^2
vector_t bb = poly_power(b,2);
printf("\nCoefficients of polynomial b times itself\n");
print_vector(bb);
// clean up all the allocated memory. This isn't strictly necessary since
// we are already at the end of the program, but good practice to do.
destroy_matrix(&A);
destroy_matrix(&AA);
destroy_vector(&b);
destroy_vector(&bb);
destroy_vector(&x);
destroy_vector(&xqr);
destroy_matrix(&Q);
destroy_matrix(&R);
printf("DONE\n");
return 0;
}
void run_no_events()
{
initialize_matrices() ;
multiply_matrices() ;
}
void update_view( player_data_t *plyr, scalar_t dt )
{
point_t view_pt;
vector_t view_dir, up_dir, vel_dir, view_vec;
scalar_t ycoord;
scalar_t course_angle;
vector_t axis;
matrixgl_t rot_mat;
vector_t y_vec;
vector_t mz_vec;
vector_t vel_proj;
quaternion_t rot_quat;
scalar_t speed;
vector_t vel_cpy;
scalar_t time_constant_mult;
vel_cpy = plyr->vel;
speed = normalize_vector( &vel_cpy );
time_constant_mult = 1.0 /
min( 1.0,
max( 0.0,
( speed - NO_INTERPOLATION_SPEED ) /
( BASELINE_INTERPOLATION_SPEED - NO_INTERPOLATION_SPEED )));
up_dir = make_vector( 0, 1, 0 );
vel_dir = plyr->vel;
normalize_vector( &vel_dir );
course_angle = get_course_angle();
switch( plyr->view.mode ) {
case TUXEYE:
{
scalar_t f = 2;
vector_t v = plyr->plane_nml;
scalar_t n = 1.;
view_pt = plyr->pos;
view_pt.x += v.x / n * 0.3;
view_pt.y += v.y / n * 0.3;
view_pt.y += 0.1;
view_pt.z += v.z / n * 0.3;
if(plyr->control.flip_factor || plyr->control.barrel_roll_factor) {
matrixgl_t mat1, mat;
vector_t right;
scalar_t n = sqrt(plyr->viewdir_for_tuxeye.x * plyr->viewdir_for_tuxeye.x + plyr->viewdir_for_tuxeye.y * plyr->viewdir_for_tuxeye.y + plyr->viewdir_for_tuxeye.z * plyr->viewdir_for_tuxeye.z);
plyr->viewdir_for_tuxeye.x /= n;
plyr->viewdir_for_tuxeye.y /= n;
plyr->viewdir_for_tuxeye.z /= n;
n = sqrt(plyr->updir_for_tuxeye.x * plyr->updir_for_tuxeye.x + plyr->updir_for_tuxeye.y * plyr->updir_for_tuxeye.y + plyr->updir_for_tuxeye.z * plyr->updir_for_tuxeye.z);
plyr->updir_for_tuxeye.x /= n;
plyr->updir_for_tuxeye.y /= n;
plyr->updir_for_tuxeye.z /= n;
right = cross_product(plyr->updir_for_tuxeye, plyr->viewdir_for_tuxeye);
make_rotation_about_vector_matrix( mat1, right, jump_from_time(plyr->control.flip_factor) * 360 );
make_rotation_about_vector_matrix( mat, plyr->viewdir_for_tuxeye, jump_from_time(plyr->control.barrel_roll_factor) * 360 );
multiply_matrices(mat, mat1, mat);
view_dir = transform_vector(mat, plyr->viewdir_for_tuxeye);
up_dir = transform_vector(mat, plyr->updir_for_tuxeye);
}
else {
view_dir = plyr->direction;
view_dir.y += 0.1;
view_dir.x = (plyr->view.dir.x * f + view_dir.x) / (f + 1);
view_dir.y = (plyr->view.dir.y * f + view_dir.y) / (f + 1);
view_dir.z = (plyr->view.dir.z * f + view_dir.z) / (f + 1);
plyr->viewdir_for_tuxeye = view_dir;
up_dir = plyr->plane_nml;
up_dir.x = (plyr->view.up.x * f + up_dir.x) / (f + 1);
up_dir.y = (plyr->view.up.y * f + up_dir.y) / (f + 1);
up_dir.z = (plyr->view.up.z * f + up_dir.z) / (f + 1);
plyr->updir_for_tuxeye = up_dir;
}
break;
}
case BEHIND:
{
/* Camera-on-a-string mode */
/* Construct vector from player to camera */
view_vec = make_vector( 0,
sin( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE +
PLAYER_ANGLE_IN_CAMERA ) ),
cos( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE +
PLAYER_ANGLE_IN_CAMERA ) ) );
view_vec = scale_vector( CAMERA_DISTANCE, view_vec );
y_vec = make_vector( 0.0, 1.0, 0.0 );
mz_vec = make_vector( 0.0, 0.0, -1.0 );
vel_proj = project_into_plane( y_vec, vel_dir );
normalize_vector( &vel_proj );
/* Rotate view_vec so that it places the camera behind player */
rot_quat = make_rotation_quaternion( mz_vec, vel_proj );
view_vec = rotate_vector( rot_quat, view_vec );
/* Construct view point */
view_pt = move_point( plyr->pos, view_vec );
/* Make sure view point is above terrain */
ycoord = find_y_coord( view_pt.x, view_pt.z );
if ( view_pt.y < ycoord + MIN_CAMERA_HEIGHT ) {
view_pt.y = ycoord + MIN_CAMERA_HEIGHT;
}
/* Interpolate view point */
if ( plyr->view.initialized ) {
/* Interpolate twice to get a second-order filter */
int i;
for (i=0; i<2; i++) {
view_pt =
interpolate_view_pos( plyr->pos, plyr->pos,
MAX_CAMERA_PITCH, plyr->view.pos,
view_pt, CAMERA_DISTANCE, dt,
BEHIND_ORBIT_TIME_CONSTANT *
time_constant_mult );
}
}
/* Make sure interpolated view point is above terrain */
ycoord = find_y_coord( view_pt.x, view_pt.z );
if ( view_pt.y < ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT ) {
view_pt.y = ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT;
}
/* Construct view direction */
view_vec = subtract_points( view_pt, plyr->pos );
axis = cross_product( y_vec, view_vec );
normalize_vector( &axis );
make_rotation_about_vector_matrix( rot_mat, axis,
PLAYER_ANGLE_IN_CAMERA );
view_dir = scale_vector( -1.0,
transform_vector( rot_mat, view_vec ) );
/* Interpolate orientation of camera */
if ( plyr->view.initialized ) {
/* Interpolate twice to get a second-order filter */
int i;
for (i=0; i<2; i++) {
interpolate_view_frame( plyr->view.up, plyr->view.dir,
&up_dir, &view_dir, dt,
BEHIND_ORIENT_TIME_CONSTANT );
up_dir = make_vector( 0.0, 1.0, 0.0 );
}
}
break;
}
case FOLLOW:
{
/* Camera follows player (above and behind) */
up_dir = make_vector( 0, 1, 0 );
/* Construct vector from player to camera */
view_vec = make_vector( 0,
sin( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE +
PLAYER_ANGLE_IN_CAMERA ) ),
cos( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE +
PLAYER_ANGLE_IN_CAMERA ) ) );
view_vec = scale_vector( CAMERA_DISTANCE, view_vec );
y_vec = make_vector( 0.0, 1.0, 0.0 );
mz_vec = make_vector( 0.0, 0.0, -1.0 );
vel_proj = project_into_plane( y_vec, vel_dir );
normalize_vector( &vel_proj );
/* Rotate view_vec so that it places the camera behind player */
rot_quat = make_rotation_quaternion( mz_vec, vel_proj );
view_vec = rotate_vector( rot_quat, view_vec );
/* Construct view point */
view_pt = move_point( plyr->pos, view_vec );
/* Make sure view point is above terrain */
ycoord = find_y_coord( view_pt.x, view_pt.z );
if ( view_pt.y < ycoord + MIN_CAMERA_HEIGHT ) {
view_pt.y = ycoord + MIN_CAMERA_HEIGHT;
}
/* Interpolate view point */
if ( plyr->view.initialized ) {
/* Interpolate twice to get a second-order filter */
int i;
for ( i=0; i<2; i++ ) {
view_pt =
interpolate_view_pos( plyr->view.plyr_pos, plyr->pos,
MAX_CAMERA_PITCH, plyr->view.pos,
view_pt, CAMERA_DISTANCE, dt,
FOLLOW_ORBIT_TIME_CONSTANT *
time_constant_mult );
}
}
/* Make sure interpolate view point is above terrain */
ycoord = find_y_coord( view_pt.x, view_pt.z );
if ( view_pt.y < ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT ) {
view_pt.y = ycoord + ABSOLUTE_MIN_CAMERA_HEIGHT;
}
/* Construct view direction */
view_vec = subtract_points( view_pt, plyr->pos );
axis = cross_product( y_vec, view_vec );
normalize_vector( &axis );
make_rotation_about_vector_matrix( rot_mat, axis,
PLAYER_ANGLE_IN_CAMERA );
view_dir = scale_vector( -1.0,
transform_vector( rot_mat, view_vec ) );
/* Interpolate orientation of camera */
if ( plyr->view.initialized ) {
/* Interpolate twice to get a second-order filter */
int i;
for ( i=0; i<2; i++ ) {
interpolate_view_frame( plyr->view.up, plyr->view.dir,
&up_dir, &view_dir, dt,
FOLLOW_ORIENT_TIME_CONSTANT );
up_dir = make_vector( 0.0, 1.0, 0.0 );
}
}
break;
}
case ABOVE:
{
/* Camera always uphill of player */
up_dir = make_vector( 0, 1, 0 );
/* Construct vector from player to camera */
view_vec = make_vector( 0,
sin( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE+
PLAYER_ANGLE_IN_CAMERA ) ),
cos( ANGLES_TO_RADIANS(
course_angle -
CAMERA_ANGLE_ABOVE_SLOPE+
PLAYER_ANGLE_IN_CAMERA ) ) );
view_vec = scale_vector( CAMERA_DISTANCE, view_vec );
/* Construct view point */
view_pt = move_point( plyr->pos, view_vec );
/* Make sure view point is above terrain */
ycoord = find_y_coord( view_pt.x, view_pt.z );
if ( view_pt.y < ycoord + MIN_CAMERA_HEIGHT ) {
view_pt.y = ycoord + MIN_CAMERA_HEIGHT;
}
/* Construct view direction */
view_vec = subtract_points( view_pt, plyr->pos );
make_rotation_matrix( rot_mat, PLAYER_ANGLE_IN_CAMERA, 'x' );
view_dir = scale_vector( -1.0,
transform_vector( rot_mat, view_vec ) );
break;
}
default:
code_not_reached();
}
/* Create view matrix */
plyr->view.pos = view_pt;
plyr->view.dir = view_dir;
plyr->view.up = up_dir;
plyr->view.plyr_pos = plyr->pos;
plyr->view.initialized = True;
setup_view_matrix( plyr );
}
