/* TODO: Needs to be optimized? */
int moto_ray_intersect_triangle(MotoRay *self,
MotoIntersection *intersection,
float A[3], float B[3], float C[3])
{
float tmp;
float normal[3], v1[3], v2[3];
vector3_dif(v1, B, A);
vector3_dif(v2, C, A);
vector3_cross(normal, v1, v2);
vector3_normalize(normal, tmp);
if( ! moto_ray_intersect_plane(self, intersection, A, normal))
return 0;
float n1[3], n2[3], n3[3], oA[3], oB[3], oC[3];
vector3_dif(oA, A, self->pos);
vector3_dif(oB, B, self->pos);
vector3_dif(oC, C, self->pos);
if(vector3_dot(self->dir, normal) < 0) /* faceforward */
{
vector3_cross(n1, oC, oA);
vector3_normalize(n1, tmp);
if(vector3_dot(n1, self->dir) > 0)
return 0;
vector3_cross(n2, oB, oC);
vector3_normalize(n2, tmp);
if(vector3_dot(n2, self->dir) > 0)
return 0;
vector3_cross(n3, oA, oB);
vector3_normalize(n3, tmp);
if(vector3_dot(n3, self->dir) > 0)
return 0;
}
else
{
vector3_cross(n1, oA, oC);
vector3_normalize(n1, tmp);
if(vector3_dot(n1, self->dir) > 0)
return 0;
vector3_cross(n2, oC, oB);
vector3_normalize(n2, tmp);
if(vector3_dot(n2, self->dir) > 0)
return 0;
vector3_cross(n3, oB, oA);
vector3_normalize(n3, tmp);
if(vector3_dot(n3, self->dir) > 0)
return 0;
}
return 1;
}
void vector3_test()
{
vector3 zero = {0,0,0};
vector3 one = {1,1,1};
vector3 y = {0,1,0};
vector3 half = {0.5,0.5,0.5};
vector3 a;
vector3_invert(&a, &one);
vector3_subtract(&a, &one, &a);
vector3_add(&a, &a, &one);
vector3_print(&a);
vector3_multiply(&a, &one, 0.5);
vector3_divide(&a, &a, 2);
vector3_print(&a);
vector3_reflect(&a, &one, &y);
vector3_print(&a);
vector3_scalar_sub(&a, &zero, -0.5);
vector3_scalar_add(&a, &a, 0.5);
vector3_print(&a);
vector3_cross(&a, &one, &y);
vector3_print(&a);
srand(3);
vector3_random(&a);
vector3_print(&a);
printf("%.2f %.2f\n",
vector3_dot(&half, &y), vector3_angle(&half, &y));
printf("%.2f %.2f\n",
vector3_distance(&one, &y), vector3_distancesq(&one, &y));
vector3_copy(&a, &one);
printf("%.2f %.2f\n",
vector3_length(&one), vector3_length(vector3_normalize(&a)) );
}
static void update_enemies_path()
{
vector3_t v;
u32b_t i;
// Update position of each enemy that has "started"
for(i=0; i<State->nenemies; ) {
// Move enemy towards next path point
if( (State->enemies[i].start_time < State->time) && (State->enemies[i].path != State->path) ) {
// Build vector poiting toward waypoint
vector3_sub_vector(&State->enemies[i].path->position, &State->enemies[i].position, &v);
// If waypoint is in reach, simply jump there and assign next node
if( vector3_length(&v) < State->enemies[i].speed*BASE_SPEED ) {
vector3_copy(&State->enemies[i].path->position, &State->enemies[i].position);
State->enemies[i].path = State->enemies[i].path->next;
// Move on to next entry
i++;
} else {
// Just move towards it
vector3_normalize(&v, &v);
vector3_mult_scalar(&v,&v,State->enemies[i].speed*BASE_SPEED);
vector3_add_vector(&State->enemies[i].position, &v, &State->enemies[i].position);
// Move on to next entry
i++;
}
} else if( State->enemies[i].path == State->path ) {
// Enemy reached last path node!
// Remove points from mana.
State->player.mana -= State->enemies[i].health*0.1;
// Notify GUI of the player hit.
gui_game_event_hit(State->time,&State->enemies[i]);
// Kill it (will move last entry into current); retest current.
enemy_kill_enemy(i, State->enemies, &State->nenemies, State->senemies);
} else {
// Move on to next entry.
i++;
}
}
}
void moto_object_node_tumble_v(MotoObjectNode *self, gfloat da)
{
da = da*RAD_PER_DEG;
gfloat ax[] = {1, 0, 0};
gfloat ay[] = {0, 1, 0};
gfloat az[] = {0, 0, 1};
gfloat u[3], v[3], n[3], t[3];
gfloat to_u[3], to_v[3], to_n[3];
gfloat to_eye[3], eye[3];
gfloat loc_pos[] = {0, 0, 0};
gfloat tumble_axis[] = {0, 1, 0};
gfloat target[3];
vector3_copy(target, self->priv->target);
gfloat *matrix = moto_object_node_get_matrix(self, TRUE);
vector3_transform(u, matrix, ax);
vector3_transform(v, matrix, ay);
vector3_transform(n, matrix, az);
point3_transform(eye, matrix, loc_pos);
vector3_dif(to_eye, eye, target);
vector3_sum(to_u, to_eye, u);
vector3_sum(to_v, to_eye, v);
vector3_sum(to_n, to_eye, n);
gfloat rm[16];
matrix44_rotate_from_axis(rm, da, tumble_axis[0], tumble_axis[1], tumble_axis[2]);
/* eye rotation */
gfloat to_eye2[3];
point3_transform(to_eye2, rm, to_eye);
/* u rotation */
gfloat to_u2[3];
vector3_transform(to_u2, rm, to_u);
/* v rotation */
gfloat to_v2[3];
vector3_transform(to_v2, rm, to_v);
/* n rotation */
gfloat to_n2[3];
vector3_transform(to_n2, rm, to_n);
/* new eye, u, v, n */
vector3_sum(eye, to_eye2, target);
vector3_dif(u, to_u2, to_eye2);
vector3_dif(v, to_v2, to_eye2);
vector3_dif(n, to_n2, to_eye2);
vector3_normalize(u, t[0]);
vector3_normalize(v, t[0]);
vector3_normalize(n, t[0]);
/* inverse global matrix */
gfloat igm[16];
matrix44_camera_inverse(igm, eye, u, v, n);
/* global matrix */
gfloat gm[16], ambuf[16], detbuf;
matrix44_inverse(gm, igm, ambuf, detbuf);
if(fabs(detbuf) < MICRO)
{
moto_error("(moto_object_node_tumble) determinant is zero");
return;
}
/* local matrix */
gfloat lm[16];
gfloat *lmp = gm;
if(self->priv->parent)
{
gfloat *parent_inverse = moto_object_node_get_inverse_matrix(self->priv->parent, TRUE);
matrix44_mult(lm, parent_inverse, gm);
lmp = lm;
}
gfloat translate[3];
translate_from_matrix44(translate, lmp);
moto_object_node_set_translate_array(self, translate);
gfloat euler[3], cosbuf;
switch(self->priv->rotate_order)
{
case MOTO_ROTATE_ORDER_XYZ:
euler_xyz_from_matrix44(euler, lmp, cosbuf);
break;
case MOTO_ROTATE_ORDER_XZY:
euler_xzy_from_matrix44(euler, lmp, cosbuf);
break;
case MOTO_ROTATE_ORDER_YXZ:
euler_yxz_from_matrix44(euler, lmp, cosbuf);
break;
case MOTO_ROTATE_ORDER_YZX:
euler_yzx_from_matrix44(euler, lmp, cosbuf);
break;
case MOTO_ROTATE_ORDER_ZXY:
euler_zxy_from_matrix44(euler, lmp, cosbuf);
break;
case MOTO_ROTATE_ORDER_ZYX:
euler_zyx_from_matrix44(euler, lmp, cosbuf);
break;
}
euler[0] *= DEG_PER_RAD;
euler[1] *= DEG_PER_RAD;
euler[2] *= DEG_PER_RAD;
moto_object_node_set_rotate_array(self, euler);
}
/**
* @ingroup vector3
* @brief finds the vector describing the normal between two vectors
*
*/
HYPAPI vector3 *vector3_find_normal_axis_between(vector3 *vR, const vector3 *vT1, const vector3 *vT2)
{
vector3_cross_product(vR, vT1, vT2);
vector3_normalize(vR);
return vR;
}
void helicalTurnCntrl(void)
{
union longww accum;
int16_t pitchAdjustAngleOfAttack;
int16_t rollErrorVector[3];
int16_t rtlkick;
int16_t desiredPitch;
int16_t steeringInput;
int16_t desiredTiltVector[3];
int16_t desiredRotationRateGyro[3];
uint16_t airSpeed;
union longww desiredTilt;
int16_t desiredPitchVector[2];
int16_t desiredPerpendicularPitchVector[2];
int16_t actualPitchVector[2];
int16_t pitchDot;
int16_t pitchCross;
int16_t pitchError;
int16_t pitchEarthBodyProjection[2];
int16_t angleOfAttack;
#ifdef TestGains
state_flags._.GPS_steering = 0; // turn off navigation
state_flags._.pitch_feedback = 1; // turn on stabilization
airSpeed = 981; // for testing purposes, an airspeed is needed
#else
airSpeed = air_speed_3DIMU;
if (airSpeed < TURN_CALC_MINIMUM_AIRSPEED) airSpeed = TURN_CALC_MINIMUM_AIRSPEED;
#endif
// determine the desired turn rate as the sum of navigation and fly by wire.
// this allows the pilot to override navigation if needed.
steeringInput = 0 ; // just in case no airframe type is specified or radio is off
if (udb_flags._.radio_on == 1)
{
#if ( (AIRFRAME_TYPE == AIRFRAME_STANDARD) || (AIRFRAME_TYPE == AIRFRAME_GLIDER) )
if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about this
{
steeringInput = udb_pwIn[ AILERON_INPUT_CHANNEL ] - udb_pwTrim[ AILERON_INPUT_CHANNEL ];
steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, steeringInput);
}
else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED)
{
steeringInput = udb_pwIn[ RUDDER_INPUT_CHANNEL ] - udb_pwTrim[ RUDDER_INPUT_CHANNEL ];
steeringInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, steeringInput);
}
else
{
steeringInput = 0;
}
#endif // AIRFRAME_STANDARD
#if (AIRFRAME_TYPE == AIRFRAME_VTAIL)
// use aileron channel if it is available, otherwise use rudder
if (AILERON_INPUT_CHANNEL != CHANNEL_UNUSED) // compiler is smart about this
{
steeringInput = udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL];
steeringInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, steeringInput);
}
else if (RUDDER_INPUT_CHANNEL != CHANNEL_UNUSED)
{
// unmix the Vtail
int16_t rudderInput = REVERSE_IF_NEEDED(RUDDER_CHANNEL_REVERSED, (udb_pwIn[ RUDDER_INPUT_CHANNEL] - udb_pwTrim[RUDDER_INPUT_CHANNEL]));
int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, (udb_pwIn[ ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
steeringInput = (-rudderInput + elevatorInput);
}
else
{
steeringInput = 0;
}
#endif // AIRFRAME_VTAIL
#if (AIRFRAME_TYPE == AIRFRAME_DELTA)
// delta wing must have an aileron input, so use that
// unmix the elevons
int16_t aileronInput = REVERSE_IF_NEEDED(AILERON_CHANNEL_REVERSED, (udb_pwIn[AILERON_INPUT_CHANNEL] - udb_pwTrim[AILERON_INPUT_CHANNEL]));
int16_t elevatorInput = REVERSE_IF_NEEDED(ELEVATOR_CHANNEL_REVERSED, (udb_pwIn[ELEVATOR_INPUT_CHANNEL] - udb_pwTrim[ELEVATOR_INPUT_CHANNEL]));
steeringInput = REVERSE_IF_NEEDED(ELEVON_VTAIL_SURFACES_REVERSED, ((elevatorInput - aileronInput)));
#endif // AIRFRAME_DELTA
}
if (steeringInput > MAX_INPUT) steeringInput = MAX_INPUT;
if (steeringInput < - MAX_INPUT) steeringInput = - MAX_INPUT;
// note that total steering is the sum of pilot input and waypoint navigation,
// so that the pilot always has some say in the matter
accum.WW = __builtin_mulsu(steeringInput, turngainfbw) /(2*MAX_INPUT);
if ((settings._.AileronNavigation || settings._.RudderNavigation) && state_flags._.GPS_steering)
{
accum.WW +=(int32_t) navigate_determine_deflection('t');
}
if (accum.WW >(int32_t) 2*(int32_t) RMAX - 1) accum.WW =(int32_t) 2*(int32_t) RMAX - 1;
if (accum.WW < -(int32_t) 2*(int32_t) RMAX + 1) accum.WW = -(int32_t) 2*(int32_t) RMAX + 1;
desiredTurnRateRadians = accum._.W0;
// compute the desired tilt from desired turn rate and air speed
// range for acceleration is plus minus 4 times gravity
// range for turning rate is plus minus 4 radians per second
// desiredTilt is the ratio(-rmat[6]/rmat[8]), times RMAX/2 required for the turn
// desiredTilt = desiredTurnRate * airSpeed / gravity
// desiredTilt = RMAX/2*"real desired tilt"
// desiredTurnRate = RMAX/2*"real desired turn rate", desired turn rate in radians per second
// airSpeed is air speed centimeters per second
// gravity is 981 centimeters per second per second
desiredTilt.WW = - __builtin_mulsu(desiredTurnRateRadians, airSpeed);
desiredTilt.WW /= GRAVITYCMSECSEC;
// limit the lateral acceleration to +- 4 times gravity, total wing loading approximately 4.12 times gravity
if (desiredTilt.WW > (int32_t)2 * (int32_t)RMAX - 1)
{
desiredTilt.WW = (int32_t)2 * (int32_t)RMAX - 1;
accum.WW = __builtin_mulsu(-desiredTilt._.W0, GRAVITYCMSECSEC);
accum.WW /= airSpeed;
desiredTurnRateRadians = accum._.W0;
}
else if (desiredTilt.WW < -(int32_t)2 * (int32_t)RMAX + 1)
{
desiredTilt.WW = -(int32_t)2 * (int32_t)RMAX + 1;
accum.WW = __builtin_mulsu(-desiredTilt._.W0, GRAVITYCMSECSEC);
accum.WW /= airSpeed;
desiredTurnRateRadians = accum._.W0;
}
// Compute the amount of lift needed to perform the desired turn
// Tests show that the best estimate of lift is obtained using
// actual values of rmat[6] and rmat[8], and the commanded value of their ratio
estimatedLift = wingLift(rmat[6], rmat[8], desiredTilt._.W0);
// compute angle of attack and elevator trim based on relative wing loading.
// relative wing loading is the ratio of wing loading divided by the stall wing loading, as a function of air speed
// both angle of attack and trim are computed by a linear approximation as a function of relative loading:
// y = (2m)*(x/2) + b, y is either angle of attack or elevator trim.
// x is relative wing loading. (x/2 is computed instead of x)
// 2m and b are determined from values of angle of attack and trim at stall speed, normal and inverted.
// b = (y_normal + y_inverted) / 2.
// 2m = (y_normal - y_inverted).
// If airspeed is greater than stall speed, compute angle of attack and elevator trim,
// otherwise set AoA and trim to zero.
if (air_speed_3DIMU > STALL_SPEED_CM_SEC)
{
// compute "x/2", the relative wing loading
relativeLoading = relativeWingLoading(estimatedLift, air_speed_3DIMU);
// multiply x/2 by 2m for angle of attack
accum.WW = __builtin_mulss(AOA_SLOPE, relativeLoading);
// add mx to b
angleOfAttack = AOA_OFFSET + accum._.W1;
// project angle of attack into the earth frame
accum.WW =(__builtin_mulss(angleOfAttack, rmat[8])) << 2;
pitchAdjustAngleOfAttack = accum._.W1;
// similarly, compute elevator trim
accum.WW = __builtin_mulss(ELEVATOR_TRIM_SLOPE, relativeLoading);
elevatorLoadingTrim = ELEVATOR_TRIM_OFFSET + accum._.W1;
}
else
{
angleOfAttack = 0;
pitchAdjustAngleOfAttack = 0;
elevatorLoadingTrim = 0;
}
// SetAofA(angleOfAttack); // removed by helicalTurns
// convert desired turn rate from radians/second to gyro units
accum.WW = (((int32_t)desiredTurnRateRadians) << 4); // desired turn rate in radians times 16 to provide resolution for the divide to follow
accum.WW = accum.WW / RADSTOGYRO; // at this point accum._.W0 has 2 times the required gyro signal for the turn.
// compute desired rotation rate vector in body frame, scaling is same as gyro signal
VectorScale(3, desiredRotationRateGyro, &rmat[6], accum._.W0); // this operation has side effect of dividing by 2
// compute desired rotation rate vector in body frame, scaling is in RMAX/2*radians/sec
VectorScale(3, desiredRotationRateRadians, &rmat[6], desiredTurnRateRadians); // this produces half of what we want
VectorAdd(3, desiredRotationRateRadians, desiredRotationRateRadians, desiredRotationRateRadians); // double
// incorporate roll into desired tilt vector
desiredTiltVector[0] = desiredTilt._.W0;
desiredTiltVector[1] = 0;
desiredTiltVector[2] = RMAX/2; // the divide by 2 is to account for the RMAX/2 scaling in both tilt and rotation rate
vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure tilt vector has magnitude RMAX
// incorporate pitch into desired tilt vector
// compute return to launch pitch down kick for unpowered RTL
if (!udb_flags._.radio_on && state_flags._.GPS_steering)
{
rtlkick = RTLKICK;
}
else
{
rtlkick = 0;
}
// Compute Matt's glider pitch adjustment
#if (GLIDE_AIRSPEED_CONTROL == 1)
fractional aspd_pitch_adj = gliding_airspeed_pitch_adjust();
#endif
// Compute total desired pitch
#if (GLIDE_AIRSPEED_CONTROL == 1)
desiredPitch = - rtlkick + aspd_pitch_adj + pitchAltitudeAdjust;
#else
desiredPitch = - rtlkick + pitchAltitudeAdjust;
#endif
// Adjustment for inverted flight
if (!canStabilizeInverted() || !desired_behavior._.inverted)
{
// normal flight
desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack;
}
else
{
// inverted flight
desiredTiltVector[0] = - desiredTiltVector[0];
desiredTiltVector[1] = - desiredPitch - pitchAdjustAngleOfAttack - INVNPITCH; // only one of the adjustments is not zero
desiredTiltVector[2] = - desiredTiltVector[2];
}
vector3_normalize(desiredTiltVector, desiredTiltVector); // make sure tilt vector has magnitude RMAX
// compute roll error
VectorCross(rollErrorVector, &rmat[6], desiredTiltVector); // compute tilt orientation error
if (VectorDotProduct(3, &rmat[6], desiredTiltVector) < 0) // more than 90 degree error
{
vector3_normalize(rollErrorVector, rollErrorVector); // for more than 90 degrees, make the tilt error vector parallel to desired axis, with magnitude RMAX
}
tiltError[1] = rollErrorVector[1];
// compute pitch error
// start by computing the projection of earth frame pitch error to body frame
pitchEarthBodyProjection[0] = rmat[6];
pitchEarthBodyProjection[1] = rmat[8];
// normalize the projection vector and compute the cosine of the actual pitch as a side effect
actualPitchVector[1] =(int16_t) vector2_normalize(pitchEarthBodyProjection, pitchEarthBodyProjection);
// complete the actual pitch vector
actualPitchVector[0] = rmat[7];
// compute the desired pitch vector
desiredPitchVector[0] = - desiredPitch;
desiredPitchVector[1] = RMAX;
vector2_normalize(desiredPitchVector, desiredPitchVector);
// rotate desired pitch vector by 90 degrees to be able to compute cross product using VectorDot
desiredPerpendicularPitchVector[0] = desiredPitchVector[1];
desiredPerpendicularPitchVector[1] = - desiredPitchVector[0];
// compute pitchDot, the dot product of actual and desired pitch vector
// (the 2* that appears in several of the following expressions is a result of the Q2.14 format)
pitchDot = 2*VectorDotProduct(2, actualPitchVector, desiredPitchVector);
// compute pitchCross, the cross product of the actual and desired pitch vector
pitchCross = 2*VectorDotProduct(2, actualPitchVector, desiredPerpendicularPitchVector);
if (pitchDot > 0)
{
pitchError = pitchCross;
}
else
{
if (pitchCross > 0)
{
pitchError = RMAX;
}
else
{
pitchError = - RMAX;
}
}
// multiply the normalized rmat[6], rmat[8] vector by the pitch error
VectorScale(2, pitchEarthBodyProjection, pitchEarthBodyProjection, pitchError);
tiltError[0] = 2*pitchEarthBodyProjection[1];
tiltError[2] = - 2*pitchEarthBodyProjection[0];
// compute the rotation rate error vector
VectorSubtract(3, rotationRateError, omegaAccum, desiredRotationRateGyro);
}
void moto_ray_normalize(MotoRay *self)
{
float lenbuf;
vector3_normalize(self->dir, lenbuf);
}
int moto_ray_intersect_sphere_2(MotoRay *self,
MotoIntersection *intersection,
float origin[3], float square_radius)
{
float tmp, dst[3];
vector3_dif(dst, self->pos, origin);
float B = vector3_dot(dst, self->dir);
float C = vector3_dot(dst, dst) - square_radius;
float D = B*B - C;
if(D > MICRO)
{
float sqrtD = sqrt(D);
intersection->hits_num = 0;
intersection->hits[0].dist = sqrtD - B;
if(intersection->hits[0].dist > MICRO)
{
vector3_copy(intersection->hits[0].point, self->pos);
point3_move(intersection->hits[0].point, self->dir, intersection->hits[0].dist);
vector3_dif(intersection->hits[0].normal, intersection->hits[0].point, origin);
vector3_normalize(intersection->hits[0].normal, tmp);
intersection->hits[0].is_entering = \
(vector3_dot(intersection->hits[0].normal, self->dir) < 0);
intersection->hits_num++;
}
intersection->hits[intersection->hits_num].dist = -sqrtD - B;
if(intersection->hits[intersection->hits_num].dist > MICRO)
{
vector3_copy(intersection->hits[intersection->hits_num].point, self->pos);
point3_move(intersection->hits[intersection->hits_num].point, self->dir,
intersection->hits[intersection->hits_num].dist);
vector3_dif(intersection->hits[intersection->hits_num].normal,
intersection->hits[intersection->hits_num].point, origin);
vector3_normalize(intersection->hits[intersection->hits_num].normal, tmp);
intersection->hits[intersection->hits_num].is_entering = \
(vector3_dot(intersection->hits[intersection->hits_num].normal, self->dir) < 0);
intersection->hits_num++;
}
return intersection->hits_num > 0;
}
else if(D >= 0 && D < MICRO)
{
intersection->hits_num = 0;
intersection->hits[0].dist = -B;
if(-B < MICRO)
{
vector3_copy(intersection->hits[0].point, self->pos);
point3_move(intersection->hits[0].point, self->dir,
intersection->hits[0].dist);
vector3_dif(intersection->hits[0].normal, intersection->hits[0].point, origin);
vector3_normalize(intersection->hits[0].normal, tmp);
intersection->hits[0].is_entering = 0;
intersection->hits_num = 1;
}
return intersection->hits_num > 0;
}
return 0;
}
