fly.c
2.44 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
/*
* Copyright 1991, 1992, 1993, 1994, 1995 Silicon Graphics, Inc.
* All Rights Reserved.
*
* This is UNPUBLISHED PROPRIETARY SOURCE CODE of Silicon Graphics, Inc.;
* the contents of this file may not be disclosed to third parties, copied or
* duplicated in any form, in whole or in part, without the prior written
* permission of Silicon Graphics, Inc.
*
*/
/*
* fly -
* Simple flying interface
*
* exports:
* fly_init();
* set_fly();
* get_altitude();
* getflytransform();
*
*/
#include "gu.h"
#include "em.h"
#include "vect.h"
#include "stress.h"
typedef struct {
float r, p, h; /* rotation */
vect pos; /* position */
float vel; /* velocity */
vect v; /* velocity vector */
} OwnShip;
static float Dt = 1 / 10.0;
static OwnShip Modl;
float
get_altitude( void )
{
return(Modl.pos.y);
}
void
fly_init( void )
{
Modl.pos.x = 0.0;
Modl.pos.y = 0.0;
Modl.pos.z = 0.0;
Modl.vel = 0.0;
Modl.v.x = 0.0;
Modl.v.y = 0.0;
Modl.v.z = 1.0;
Modl.r = 0.0;
Modl.p = 0.0;
Modl.h = 0.0;
}
/*
* velocity range -100 to 100
* roll range -180 to 180
* pitch range -180 to 180
* yaw range -180 to 180
*/
void
fly_set (int velocity, int roll, int pitch, int yaw)
{
Modl.r = (float) roll;
Modl.p = (float) pitch;
Modl.h = (float) yaw;
Modl.vel = (float) velocity;
}
/*
* Calculate rotation matrix from roll, pitch, yaw in degrees
*/
static
void RotateRPY(float mat[4][4], float r, float p, float y)
{
static float dtor = 3.1415926 / 180.0;
float sinr, sinp, siny;
float cosr, cosp, cosy;
p *= dtor; /* about z axis */
y *= dtor; /* about x axis */
r *= dtor; /* about y axis */
sinp = sinf(p);
cosp = cosf(p);
siny = sinf(y);
cosy = cosf(y);
sinr = sinf(r);
cosr = cosf(r);
mat[0][0] = cosy*cosr;
mat[0][1] = cosy*sinr;
mat[0][2] = -siny;
mat[1][0] = sinp*siny*cosr - cosp*sinr;
mat[1][1] = sinp*siny*sinr + cosp*cosr;
mat[1][2] = sinp*cosy;
mat[2][0] = cosp*siny*cosr + sinp*sinr;
mat[2][1] = cosp*siny*sinr - sinp*cosr;
mat[2][2] = cosp*cosy;
}
/*
* Calculate fly transform
*/
void
getflytransform( float mat[4][4] )
{
vect t;
myidentity(mat);
RotateRPY(mat, Modl.r, Modl.p, Modl.h);
vtransform(&Modl.v, mat, &t);
Modl.pos.x += (t.x * Modl.vel) * Dt;
Modl.pos.y += (t.y * Modl.vel) * Dt;
Modl.pos.z += (t.z * Modl.vel) * Dt;
mat[3][0] = Modl.pos.x;
mat[3][1] = Modl.pos.y;
mat[3][2] = Modl.pos.z;
}