Area Ship::bounding() const
{
// the idea in the following code is to calculate a bounding box
// around the ship's triangle and use it for collision detection.
// TODO: find a nicer way of rotating and translating
// (-center_ and +center_ is not intuitive)
// first calculate the triangle 'shadowing' the triangle on display
// this means a rotation and translation needs to be applied to the
// known points (see Ship::draw for comparison to the OpenGL code)
const vector2 &RA = A_ - center_,
&RB = B_ - center_,
&RC = C_ - center_;
// this is probably inefficient
vector2 RRA(0, 0), RRB(0, 0), RRC(0, 0);
float ra = to_radians(rot_angle_);
rotate_vector(RA, ra, RRA);
rotate_vector(RB, ra, RRB);
rotate_vector(RC, ra, RRC);
RRA += pos_ + center_;
RRB += pos_ + center_;
RRC += pos_ + center_;
const vector2* points[3] = { &RRA, &RRB, &RRC } ;
// no point storing the bounding box, because it depends on position
// find a bounding box encompassing all points
return Area::minimumArea(points, 3);
}
void _marx_transform_ray (JDMVector_Type *x, JDMVector_Type *p, /*{{{*/
_Marx_Coord_Transform_Type *a)
{
x->x -= a->dx;
x->y -= a->dy;
x->z -= a->dz;
rotate_vector (a->matrix, x);
rotate_vector (a->matrix, p);
}
int fill_planet_data( PLANET_DATA *pdata, const int planet_no, const double jd,
const double observer_lat, const double observer_lon,
const char *vsop_data)
{
double loc_sidereal_time = green_sidereal_time( jd) + observer_lon;
double t_centuries = (jd - J2000) / 36525.;
double obliquity = mean_obliquity( t_centuries);
double loc[3];
pdata->jd = jd;
if( planet_no == 10) /* get lunar data, not VSOP */
{
double fund[N_FUND];
lunar_fundamentals( vsop_data, t_centuries, fund);
lunar_lon_and_dist( vsop_data, fund, &pdata->ecliptic_lon, &pdata->r, 0L);
pdata->ecliptic_lon *= pi / 180.;
pdata->ecliptic_lat = lunar_lat( vsop_data, fund, 0L) * pi / 180.;
}
else
{
/* What we _really_ want is the location of the sun as */
/* seen from the earth. VSOP gives us the opposite, */
/* i.e., where the _earth_ is as seen from the _sun_. */
/* To evade this, we add PI to the longitude and */
/* negate the latitude. */
pdata->ecliptic_lon =
calc_vsop_loc( vsop_data, planet_no, 0, t_centuries, 0.) + pi;
pdata->ecliptic_lat =
-calc_vsop_loc( vsop_data, planet_no, 1, t_centuries, 0.);
pdata->r = calc_vsop_loc( vsop_data, planet_no, 2, t_centuries, 0.);
}
polar3_to_cartesian( loc, pdata->ecliptic_lon, pdata->ecliptic_lat);
memcpy( pdata->ecliptic_loc, loc, 3 * sizeof( double));
/* At this point, loc is a unit vector in ecliptic */
/* coords of date. Rotate it by 'obliquity' to get */
/* a vector in equatorial coords of date: */
rotate_vector( loc, obliquity, 0);
memcpy( pdata->equatorial_loc, loc, 3 * sizeof( double));
/* The following two rotations take us from a vector in */
/* equatorial coords of date to an alt/az vector: */
rotate_vector( loc, -loc_sidereal_time, 2);
/* printf( "LST: %lf\n", fmod( loc_sidereal_time * 180. / pi, 360.)); */
pdata->hour_angle = atan2( loc[1], loc[0]);
rotate_vector( loc, observer_lat - pi / 2., 1);
memcpy( pdata->altaz_loc, loc, 3 * sizeof( double));
return( 0);
}
void local_to_world(struct robot * robot, struct vect *world,
struct vect *local)
{
rotate_vector(world, local, &robot->position2.corrected_orientation);
struct vect out;
rotate_vector(&out, &camera_center, &robot->position2.corrected_orientation);
//printf("%f, %f, %f\n", out.x, out.y, out.z);
world->x += robot->position2.position.x;
world->y += robot->position2.position.y;
}
void object_render(object_t* object,camera_t camera)
{
const color_t white=get_color(255,255,255);
int i;
vector_t prev_point;
vector_t cur_point;
prev_point=vector_transform(vector_add(object->position,rotate_vector(object->points[object->num_points-1],object->rotation)),camera);
for(i=0;i<object->num_points;i++)
{
cur_point=vector_transform(vector_add(object->position,rotate_vector(object->points[i],object->rotation)),camera);
draw_line((unsigned int)prev_point.x,(unsigned int)prev_point.y,(unsigned int)cur_point.x,(unsigned int)cur_point.y,white);
prev_point=cur_point;
}
}
void move_object(t_cam *eye, t_obj *obj, int j)
{
if (j == -1)
{
eye->pos = translate_vector(eye->pos, obj->trans, j);
eye->pos = rotate_vector(eye->pos, obj->rot, j);
eye->v = rotate_vector(eye->v, obj->rot, j);
}
else
{
eye->pos = rotate_vector(eye->pos, obj->rot, j);
eye->pos = translate_vector(eye->pos, obj->trans, j);
eye->v = rotate_vector(eye->v, obj->rot, j);
}
}
// convert standard coordinates to half vector/difference vector coordinates
void std_coords_to_half_diff_coords(double theta_in, double fi_in, double theta_out, double fi_out,
double& theta_half,double& fi_half,double& theta_diff,double& fi_diff )
{
// compute in vector
double in_vec_z = cos(theta_in);
double proj_in_vec = sin(theta_in);
double in_vec_x = proj_in_vec*cos(fi_in);
double in_vec_y = proj_in_vec*sin(fi_in);
double in[3]= {in_vec_x,in_vec_y,in_vec_z};
normalize(in);
// compute out vector
double out_vec_z = cos(theta_out);
double proj_out_vec = sin(theta_out);
double out_vec_x = proj_out_vec*cos(fi_out);
double out_vec_y = proj_out_vec*sin(fi_out);
double out[3]= {out_vec_x,out_vec_y,out_vec_z};
normalize(out);
// compute halfway vector
double half_x = (in_vec_x + out_vec_x)/2.0f;
double half_y = (in_vec_y + out_vec_y)/2.0f;
double half_z = (in_vec_z + out_vec_z)/2.0f;
double half[3] = {half_x,half_y,half_z};
normalize(half);
// compute theta_half, fi_half
theta_half = acos(half[2]);
fi_half = atan2(half[1], half[0]);
double bi_normal[3] = {0.0, 1.0, 0.0};
double normal[3] = { 0.0, 0.0, 1.0 };
double temp[3];
double diff[3];
// compute diff vector
rotate_vector(in, normal , -fi_half, temp);
rotate_vector(temp, bi_normal, -theta_half, diff);
// compute theta_diff, fi_diff
theta_diff = acos(diff[2]);
fi_diff = atan2(diff[1], diff[0]);
}
void spacecraft_position_docked(spacecraft_t* spacecraft)
{
int i;
for(i=0;i<spacecraft->num_docks;i++)
{
dock_t* dock=spacecraft->docks+i;
if(dock->docked_spacecraft!=NULL)
{
dock_t* docked_dock=&dock->docked_spacecraft->docks[dock->docked_dock];
vector_t dock_1_position=rotate_vector(dock->position,dock->orientation);
vector_t dock_2_position=rotate_vector(docked_dock->position,docked_dock->orientation);
dock->docked_spacecraft->base.position=vector_add(spacecraft->base.position,rotate_vector(vector_add(dock_1_position,dock_2_position),spacecraft->base.rotation));
dock->docked_spacecraft->base.rotation=spacecraft->base.rotation+dock->orientation+docked_dock->orientation+M_PI;
}
}
}
int DLL_FUNC calc_ssat_loc( const double t, double DLLPTR *ssat,
const int sat_wanted, const long precision)
{
SAT_ELEMS elems;
ELEMENTS orbit;
double matrix[9];
const double t_years = (t - J2000) / 365.25;
if( precision == -1L) /* just checking version # */
return( 1);
if( sat_wanted < 0 || sat_wanted > PHOEBE)
return( -1);
elems.jd = t;
elems.sat_no = sat_wanted;
set_ssat_elems( &elems, &orbit);
setup_orbit_vectors( &orbit);
comet_posn_part_ii( &orbit, IGNORED_DOUBLE, elems.loc, NULL);
if( sat_wanted < RHEA) /* inner 4 satellites are returned in Saturnic */
{ /* coords so gotta rotate to B1950.0 */
rotate_vector( elems.loc, INCL0, 0);
rotate_vector( elems.loc, ASC_NODE0, 2);
}
/* After above, elems.loc is ecliptic 1950 coords */
rotate_vector( elems.loc, OBLIQUITY_1950, 0);
/* Now, elems.loc is equatorial 1950 coords */
#ifdef OUTPUT_IN_J2000
setup_precession( matrix, 1950., 2000);
precess_vector( matrix, elems.loc, ssat);
/* Now, ssats is equatorial J2000... */
rotate_vector( ssat, -OBLIQUITY_2000, 0);
/* And now, ssats is ecliptical J2000 */
#else
setup_precession( matrix, 1950., 2000. + t_years);
precess_vector( matrix, elems.loc, ssat);
/* Now, ssats is equatorial of epoch coords */
rotate_vector( ssat, -mean_obliquity( t_years / 100.), 0);
/* And now, ssats is ecliptical of epoch coords */
/* (which is what we really want anyway) */
#endif
return( 0);
}
static void pdf_write_arrow_appearance(fz_context *ctx, fz_buffer *buf, fz_rect *rect, float x, float y, float dx, float dy, float w)
{
float r = fz_max(1, w);
float angle = atan2f(dy, dx);
fz_point v, a, b;
v = rotate_vector(angle, 8.8f*r, 4.5f*r);
a = fz_make_point(x + v.x, y + v.y);
v = rotate_vector(angle, 8.8f*r, -4.5f*r);
b = fz_make_point(x + v.x, y + v.y);
*rect = fz_include_point_in_rect(*rect, a);
*rect = fz_include_point_in_rect(*rect, b);
*rect = fz_expand_rect(*rect, w);
fz_append_printf(ctx, buf, "%g %g m\n", a.x, a.y);
fz_append_printf(ctx, buf, "%g %g l\n", x, y);
fz_append_printf(ctx, buf, "%g %g l\n", b.x, b.y);
}
void update_balls(struct vector3d balls[], int size, float degrees) {
int i;
for (i = 0; i < size; i++) {
rotate_vector(&balls[i], degrees);
}
}
/*
* vect(x,y) must be normalized
* gives the tangent point of the tangent to ellpise(da,db,dalpha) parallel to vect(x,y)
* dalpha is in radians
* ellipse center is in (0,0)
*/
static void elliptic_tangent(double x, double y, double da, double db,
double dalpha, double *px, double *py)
{
double cosa = cos(dalpha);
double sina = sin(dalpha);
double u, v, len;
/* rotate (x,y) -dalpha radians */
rotate_vector(x, y, cosa, -sina, &x, &y);
/*u = (x + da*y/db)/2;
v = (y - db*x/da)/2; */
u = da * da * y;
v = -db * db * x;
len = da * db / sqrt(da * da * v * v + db * db * u * u);
u *= len;
v *= len;
rotate_vector(u, v, cosa, sina, px, py);
return;
}
spacecraft_dock(spacecraft_t* a,int a_dock_index,spacecraft_t* b,int b_dock_index)
{
dock_t* a_dock=a->docks+a_dock_index;
dock_t* b_dock=b->docks+b_dock_index;
a_dock->docked_spacecraft=b;
a_dock->docked_dock=b_dock_index;
b_dock->docked_spacecraft=a;
b_dock->docked_dock=a_dock_index;
b->base.position=vector_add(a->base.position,vector_subtract(rotate_vector(a_dock->position,a_dock->orientation),rotate_vector(b_dock->position,b_dock->orientation)));
b->parent=a->parent==NULL?a:a->parent;
}
void draw_arrow(vector_t centre,double angle,color_t color)
{
//Define shape
vector_t tip;
tip.x=0;
tip.y=10;
vector_t left;
left.x=-5;
left.y=5;
vector_t right;
right.x=5;
right.y=5;
//Rotate arrow
tip=vector_add(centre,rotate_vector(tip,angle));
left=vector_add(centre,rotate_vector(left,angle));
right=vector_add(centre,rotate_vector(right,angle));
draw_line((unsigned int)centre.x,(unsigned int)centre.y,(unsigned int)tip.x,(unsigned int)tip.y,color);
draw_line((unsigned int)tip.x,(unsigned int)tip.y,(unsigned int)left.x,(unsigned int)left.y,color);
draw_line((unsigned int)tip.x,(unsigned int)tip.y,(unsigned int)right.x,(unsigned int)right.y,color);
}
void rotate_on_camera(struct turret *turret, struct vect *result,
struct vect* input)
{
struct vect diff;
rotate_vector(&diff, input, &turret->turret_rotation);
//printf("%f, %f, %f\n", diff.x, diff.y, diff.z);
result->x = diff.x + camera_center.x;
result->y = diff.y + camera_center.y;
result->z = diff.z + camera_center.z;
}
void ViewManager::mouseMotion(int x, int y){
int centerX = hWidth;
int centerY = hHeight;
glutWarpPointer(centerX, centerY);
dir = rotate_vector(dir, vector3(0.0, 1.0, 0.0), -((float)(x-centerX))*0.002);
side = cross(-dir, up);
side.normalize();
dir1 += -0.001*(float)(y-centerY);
at = pos + dir;
at[1] += dir1;
}
void world_to_local(struct robot * robot, struct vect *local,
struct vect *world)
{
struct quaternion backwards;
struct vect temp = *world;
temp.x -= robot->position2.position.x;
temp.y -= robot->position2.position.y;
get_conjugate(&backwards, &robot->position2.corrected_orientation);
rotate_vector(local, &temp, &backwards);
}
/*!
Interpolates between camera positions.
\arg \c plyr_pos1 \c pos1 is relative to this position
\arg \c plyr_pos2 \c pos2 is relative to this position
\arg \c max_vec_angle Maximum downward pitch of vector from camera to player
\arg \c pos1 original camera position
\arg \c pos2 target camera position
\arg \c dist distance of interpolated camera position from player
\arg \c dt time step size (s)
\arg \c time_constant time constant to use in interpolation (s)
\return Interpolated camera position
\author jfpatry
\date Created: 2000-08-26
\date Modified: 2000-08-26
*/
point_t interpolate_view_pos( point_t plyr_pos1, point_t plyr_pos2,
scalar_t max_vec_angle,
point_t pos1, point_t pos2,
scalar_t dist, scalar_t dt,
scalar_t time_constant )
{
static vector_t y_vec = { 0.0, 1.0, 0.0 };
quaternion_t q1, q2;
vector_t vec1, vec2;
scalar_t alpha;
scalar_t theta;
matrixgl_t rot_mat;
vector_t axis;
vec1 = subtract_points( pos1, plyr_pos1 );
vec2 = subtract_points( pos2, plyr_pos2 );
normalize_vector( &vec1 );
normalize_vector( &vec2 );
q1 = make_rotation_quaternion( y_vec, vec1 );
q2 = make_rotation_quaternion( y_vec, vec2 );
alpha = min( MAX_INTERPOLATION_VALUE,
1.0 - exp ( -dt / time_constant ) );
q2 = interpolate_quaternions( q1, q2, alpha );
vec2 = rotate_vector( q2, y_vec );
/* Constrain angle with x-z plane */
theta = RADIANS_TO_ANGLES( M_PI/2 - acos( dot_product( vec2, y_vec ) ) );
if ( theta > max_vec_angle )
{
axis = cross_product( y_vec, vec2 );
normalize_vector( &axis );
make_rotation_about_vector_matrix( rot_mat, axis,
theta-max_vec_angle );
vec2 = transform_vector( rot_mat, vec2 );
}
return move_point( plyr_pos2,
scale_vector( dist, vec2 ) );
}
// initialise offsets to provided distance vector to other vehicle (in meters in NED frame) if required
void AP_Follow::init_offsets_if_required(const Vector3f &dist_vec_ned)
{
// return immediately if offsets have already been set
if (!_offset.get().is_zero()) {
return;
}
float target_heading_deg;
if ((_offset_type == AP_FOLLOW_OFFSET_TYPE_RELATIVE) && get_target_heading_deg(target_heading_deg)) {
// rotate offsets from north facing to vehicle's perspective
_offset = rotate_vector(-dist_vec_ned, -target_heading_deg);
} else {
// initialise offset in NED frame
_offset = -dist_vec_ned;
// ensure offset_type used matches frame of offsets saved
_offset_type = AP_FOLLOW_OFFSET_TYPE_NED;
}
}
void Ball::bounce_rotate(float amount, bool tb) // amount = [-1..1]
{
float angle;
if(tb)
{
angle = atan(_dirY / _dirX);
}
else
{
angle = atan(_dirX / _dirY);
}
angle *= (360/(2*3.1415));
angle = fabs(angle);
// a = [0..90]
// if backspin, invert angle. _dirY is already inverted
if(_dirY < 0 && _dirX > 0 && _spin > 0 ||
_dirY < 0 && _dirX < 0 && _spin < 0 ||
_dirY > 0 && _dirX > 0 && _spin < 0 ||
_dirY > 0 && _dirX < 0 && _spin > 0)
{
angle = 90 - angle;
// angle is now how much you can turn to go completely horizontal or vertical
// lessen it more for back-spins (less back-spin allowed! difficult to foresee!)
angle -= 30;
}
else
{
// angle is now how much you can turn to go completely horizontal or vertical
// lessen it for top-spins (more top-spin allowed! not as difficult to counter)
angle -= 8;
}
// angle is now how much you can turn to go completely horizontal or vertical
if(angle < 0) angle = 0;
angle *= amount;
rotate_vector(&_dirX, &_dirY, angle);
}
///
/// \brief "Fires" a round from the Ship's ammo collection. The count of
/// ammo is decremented thereafter.
/// \return void
///
void Ship::fire()
{
if (ammo_pos_ != ammo_.end()) {
vector2 bP(0, 0);
// calculate the position of the bullet at the 'nose' of the ship
float ra = to_radians(rot_angle_);
rotate_vector((C_ - center_), ra, bP);
bP += pos_ + center_;
(*ammo_pos_).fire(bP, vel_ * 5);
++ammo_pos_;
} else {
DEBUG_PRINT << ">>> OUT OF AMMO -- RELOADING!" << std::endl;
reloadAmmo();
}
}
void Triangle::draw_doodle()
{
float rotate_angle = -1;
for(int a = 0; a < 100; a++, rotate_angle-= 0.005)
{
for(int i = 0; i < 3; i++)
{
//[0] init vector from current
//[1] rotate to some angle
//[2] find intersection point of V and next line
//[3] init second point of current with inters point values
int cur = i;
int next = i+1;
if( 3 == next) next = 0;
//[0]
Line *vector = edges[cur];
//[1]
// can be negative with vx value in rotate point line:81
rotate_vector(vector, this->center, rotate_angle);
//[2]
Point *inters_point = new Point();
find_intersection_point(edges[cur], edges[next], inters_point);
//[3]
edges[next]->a = inters_point;
//try to find problem with this pointer
edges[cur]->b->x = inters_point->x;
edges[cur]->b->y = inters_point->y;
edges[cur]->draw();
// sleep(2);
usleep(10000);
}
}
}
// get offsets in meters in NED frame
bool AP_Follow::get_offsets_ned(Vector3f &offset) const
{
const Vector3f &off = _offset.get();
// if offsets are zero or type is NED, simply return offset vector
if (off.is_zero() || (_offset_type == AP_FOLLOW_OFFSET_TYPE_NED)) {
offset = off;
return true;
}
// offset type is relative, exit if we cannot get vehicle's heading
float target_heading_deg;
if (!get_target_heading_deg(target_heading_deg)) {
return false;
}
// rotate offsets from vehicle's perspective to NED
offset = rotate_vector(off, target_heading_deg);
return true;
}
void Ship::rotateVelocity()
{
//
//
// C
// *
// *|*
// * | *
// * | *
// * | *
// A * * * * * * B
// M
//
// M_C = C - M
//
// M is the mid-point between A and B
//
// The idea is to calculate the direction by calculating the
// vector M_C and rotating it
//
// ^
// ^
// ^
// C
// *^*
// * ^ *
// * ^ *
// A * * * * * B
//
//
const vector2 M((A_[0] + B_[0]) / 2, (A_[1] + B_[1]) / 2),
M_C = (C_ - M).normalize();
// use a 2D rotation matrix to rotate the M_C vector, so the ship
// goes 'forwards' with C as the front of the ship, rather than
// at some weird angle
rotate_vector(M_C, to_radians(rot_angle_), vel_);
vel_ *= speed_;
}
void get_up_vector(struct robot *robot, struct vect *result)
{
struct vect up, local_up, forward, look;
up.x = robot->position2.position.x;
up.y = robot->position2.position.y;
up.z = 1;
world_to_local(robot, &local_up, &up);
forward.x = 0;
forward.y = -1;
forward.z = 0;
struct vect normalized_look;
rotate_vector(&look, &forward, &robot->turret.turret_rotation);
// printf("%f, %f, %f\n", look.x, look.y, look.z);
float normal;
struct vect right_vector, raw;
cross(&local_up, &look, &right_vector);
// printf("%f, %f, %f\n", right_vector.x, right_vector.y, right_vector.z);
// printf("%f\n", norm(&right_vector));
cross(&right_vector, &look, &raw);
// printf("%f, %f, %f\n", raw.x, raw.y, raw.z);
normal = norm(&raw);
result->x = raw.x / normal;
result->y = raw.y / normal;
result->z = raw.z / normal;
}
/* Main program */
int main( const int argc, const char **argv)
{
const char *tle_filename = ((argc == 1) ? "test.tle" : argv[1]);
FILE *ifile = fopen( tle_filename, "rb");
tle_t tle; /* Pointer to two-line elements set for satellite */
char line1[100], line2[100];
int ephem = 1; /* default to SGP4 */
int i; /* Index for loops etc */
int n_failures = 0, n_simple = 0, n_simplex = 0;
bool failures_only = false;
for( i = 2; i < argc; i++)
if( argv[i][0] == '-')
switch( argv[i][1])
{
case 'f':
failures_only = true;
break;
case 'v':
verbose = 1;
break;
case 'd':
dist_offset = atof( argv[i] + 2);
break;
case 's':
vel_offset = atof( argv[i] + 2);
break;
default:
printf( "Option '%s' unrecognized\n", argv[i]);
break;
}
if( !ifile)
{
printf( "Couldn't open input TLE file %s\n", tle_filename);
exit( -1);
}
*line1 = '\0';
while( fgets( line2, sizeof( line2), ifile))
{
int got_data = 0;
double state_vect[6];
set_tle_defaults( &tle);
if( strlen( line2) > 110 && line2[7] == '.' && line2[18] == '.'
&& line2[0] == '2' && line2[1] == '4')
{
got_data = 3; /* Find_Orb state vector ephemeris */
tle.epoch = atof( line2);
sscanf( line2 + 13, "%lf %lf %lf %lf %lf %lf",
state_vect + 0, state_vect + 1, state_vect + 2,
state_vect + 3, state_vect + 4, state_vect + 5);
}
else if( strlen( line1) > 55 && !memcmp( line1 + 50, " (TDB)", 6))
{ /* JPL Horizons vector */
const double obliq_2000 = 23.4392911 * PI / 180.;
tle.epoch = atof( line1); /* get JD epoch from header... */
strcpy( line1, line2);
if( fgets( line2, sizeof( line2), ifile))
got_data = 1;
sscanf( line1, "%lf %lf %lf",
state_vect + 0, state_vect + 1, state_vect + 2);
sscanf( line2, "%lf %lf %lf",
state_vect + 3, state_vect + 4, state_vect + 5);
/* Cvt ecliptic to equatorial 2000: */
rotate_vector( state_vect , obliq_2000, 0);
rotate_vector( state_vect + 3, obliq_2000, 0);
}
else if( parse_elements( line1, line2, &tle) >= 0)
got_data = 2;
if( got_data == 1 || got_data == 3)
tle.epoch -= 68.00 / 86400.; /* rough convert TDT to UTC */
if( got_data) /* hey! we got a TLE! */
{
double sat_params[N_SAT_PARAMS], trial_state[6];
int simple_rval;
bool failed = false;
tle_t new_tle;
if( got_data == 1 || got_data == 3)
{
ephem = 3; /* Use SDP4 for JPL Horizons vectors */
for( i = 0; i < 6 && fabs( state_vect[i]) < 1.; i++)
;
if( i == 6) /* all small quantities, must be in AU & AU/day : */
{
for( i = 0; i < 6; i++)
state_vect[i] *= AU_IN_KM;
for( i = 3; i < 6; i++)
state_vect[i] /= seconds_per_day;
}
for( i = 3; i < 6; i++) /* cvt km/sec to km/min */
state_vect[i] *= seconds_per_minute;
if( !failures_only)
show_results( "Before:", NULL, state_vect);
}
else
{
int is_deep = select_ephemeris( &tle);
if( is_deep && (ephem == 1 || ephem == 2))
ephem += 2; /* switch to an SDx */
if( !is_deep && (ephem == 3 || ephem == 4))
ephem -= 2; /* switch to an SGx */
/* Calling of NORAD routines */
/* Each NORAD routine (SGP, SGP4, SGP8, SDP4, SDP8) */
/* will be called in turn with the appropriate TLE set */
switch( ephem)
{
case 0:
SGP_init( sat_params, &tle);
SGP( 0., &tle, sat_params, state_vect, state_vect + 3);
break;
case 1:
SGP4_init( sat_params, &tle);
SGP4( 0., &tle, sat_params, state_vect, state_vect + 3);
break;
case 2:
SGP8_init( sat_params, &tle);
SGP8( 0., &tle, sat_params, state_vect, state_vect + 3);
break;
case 3:
SDP4_init( sat_params, &tle);
SDP4( 0., &tle, sat_params, state_vect, state_vect + 3);
break;
case 4:
SDP8_init( sat_params, &tle);
SDP8( 0., &tle, sat_params, state_vect, state_vect + 3);
break;
}
if( !failures_only)
show_results( "Before:", &tle, state_vect);
}
new_tle = tle;
simple_rval = compute_tle_from_state_vector( &new_tle, state_vect, ephem, trial_state);
if( simple_rval)
{
n_simplex++;
find_tle_via_simplex_method( &new_tle, state_vect, trial_state, ephem);
}
else
n_simple++;
compute_new_state_vect( &new_tle, trial_state, ephem);
for( i = 0; i < 6; i++)
{
trial_state[i] -= state_vect[i];
if( fabs( trial_state[i]) > 1e-6)
failed = true;
}
if( failed && failures_only)
show_results( "Before:", &tle, state_vect);
if( failed || !failures_only)
show_results( (simple_rval ? "Simplex result:" : "Simplest method:"),
&new_tle, trial_state);
if( failed)
n_failures++;
}
strcpy( line1, line2);
}
fclose( ifile);
printf( "%d solved with simple method; %d with simplex\n", n_simple, n_simplex);
if( n_failures)
printf( "%d failures\n", n_failures);
return(0);
} /* End of main() */
void main(void *arg)
{
const color WHITE(1.0f);
//-----------------------------------------
const color Cs(i_color());
color diffuse_color(1.0f, 1.0f, 1.0f);
scalar diffuse_weight = i_diffuse();
color specular_color(i_specularColor());
scalar specular_weight= 0.2f;//cosinePower
scalar roughness = 0.0f;
int specular_mode = 1;//[0,3]
scalar glossiness = 1.0f;
color reflection_color(i_reflectedColor());
scalar reflection_weight = 0.5f;
color refraction_color(1.0f, 1.0f, 1.0f);
scalar refraction_weight= 0.0f;//zero will lead a dark image
scalar refraction_glossiness = 0.5f;
scalar refraction_thickness= 2.0f;//surfaceThickness
color translucency_color(i_transparency());
scalar translucency_weight = i_translucence();
scalar anisotropy = 1.0f;
scalar rotation = 0.0f;
scalar ior = 1.5f;//refractiveIndex
bool fresnel_by_ior = eiTRUE;
scalar fresnel_0_degree_refl = 0.2f;
scalar fresnel_90_degree_refl = 1.0f;
scalar fresnel_curve= 5.0f;
bool is_metal = eiTRUE;
int diffuse_samples = 8;
int reflection_samples= 4;
int refraction_samples= 4;
scalar cutoff_threshold = LIQ_SCALAR_EPSILON;
eiTag bump_shader = eiNULL_TAG;
scalar bump_factor= 0.3f;
if( liq_UserDefinedNormal() == 0 )
{
i_normalCamera() = N;
}
//-----------------------------------------
vector In = normalize( I );
normal Nn = normalize( i_normalCamera() );
normal Nf = ShadingNormal(Nn);
vector V = -In;
//-----------------------------------------
// specular is the percentage of specular lighting
// limit weights in range [0, 1]
specular_weight = clamp(specular_weight, 0.0f, 1.0f);
refraction_weight = clamp(refraction_weight, 0.0f, 1.0f);
translucency_weight = clamp(translucency_weight, 0.0f, 1.0f);
// automatically compute Kd, Ks, Kt in an energy conserving way
diffuse_weight = clamp(diffuse_weight, 0.0f, 1.0f);
reflection_weight = clamp(reflection_weight, 0.0f, 1.0f);
// the energy balancing between reflection and refraction is
// dominated by Fresnel law
//color Kt(refraction_color * (spec * refr * (1.0f - trans)) * (is_metal?Cs:WHITE));
// this is a monolithic shader which also serves as shadow shader
if (ray_type == EI_RAY_SHADOW)
{
main_shadow(arg, refraction_color * (specular_weight * refraction_weight * (1.0f - translucency_weight)),
refraction_thickness, cutoff_threshold);
return;
}
// for surface shader, we call bump shader
eiTag shader = bump_shader;
if (shader != eiNULL_TAG)
{
call_bump_shader(shader, bump_factor);
}
//color Kc(refraction_color * (spec * refr * trans) * (is_metal?Cs:WHITE));
// non-reflected energy is absorbed
//color Ks(specular_color * spec * (1.0f - refl) * (is_metal?Cs:WHITE));
//color Kr(reflection_color * (spec * refl) * (is_metal?Cs:WHITE));
// surface color will impact specular for metal material
//const color Cs(surface_color);
// const color Kd( Cs *(1.0f - spec) * diff );
// const int spec_mode = clamp(specular_mode, 0, 3);
computeSurface(
i_color(),//outColor(),//out->Ci,//
i_transparency(),//out->Oi,//
i_matteOpacityMode(),
i_matteOpacity(),
o_outColor(),//out->Ci,//
o_outTransparency()//out->Oi//
);
out->Ci = o_outColor();
out->Oi = o_outTransparency();
// apply rotation
scalar deg = rotation;
if (deg != 0.0f)
{
dPdu = rotate_vector(dPdu, N, radians(deg));
dPdv = cross(dPdu, N);
u_axis = normalize(dPdu);
v_axis = normalize(dPdv);
}
// set the glossiness scale based on the chosen BSDF
scalar glossiness_scale = 370.37f;
if (specular_mode == 1)
{
glossiness_scale = 125.0f;
}
else if (specular_mode == 3)
{
// scale to make the same glossiness parameter
// results in similar lobe for different BSDFs
glossiness_scale = 22.88f;
}
// scalar aniso = anisotropy;
int refl_samples = reflection_samples;
int refr_samples = refraction_samples;
// prepare outgoing direction in local frame
const vector wo(normalize(to_local(V)));
// construct BSDFs
OrenNayar Rd(roughness);
scalar shiny_u = glossiness;
if (shiny_u < eiSCALAR_EPS)
{
shiny_u = eiSCALAR_EPS;
refl_samples = 1;
}
shiny_u = max(0.0f, glossiness_scale / shiny_u);
scalar shiny_v = max(0.0f, shiny_u * anisotropy);
scalar IOR = ior;
eiBool fresn_by_ior = fresnel_by_ior;
scalar fresn_0_degree_refl = fresnel_0_degree_refl;
scalar fresn_90_degree_refl = fresnel_90_degree_refl;
scalar fresn_curve = fresnel_curve;
union {
eiByte by_ior[sizeof(FresnelByIOR)];
eiByte schlick[sizeof(FresnelSchlick)];
} F_storage;
union {
eiByte by_ior[sizeof(FresnelByIOR)];
eiByte schlick[sizeof(FresnelSchlick)];
} invF_storage;
Fresnel *F = NULL;
Fresnel *invF = NULL;
if (fresn_by_ior)
{
F = new (F_storage.by_ior) FresnelByIOR(IOR);
invF = new (invF_storage.by_ior) InvFresnelByIOR(IOR);
}
else
{
F = new (F_storage.schlick) FresnelSchlick(
fresn_0_degree_refl,
fresn_90_degree_refl,
fresn_curve);
invF = new (invF_storage.schlick) InvFresnelSchlick(
fresn_0_degree_refl,
fresn_90_degree_refl,
fresn_curve);
}
union {
eiByte ward[sizeof(Ward)];
eiByte phong[sizeof(StretchedPhong)];
eiByte blinn[sizeof(Blinn)];
eiByte cooktorrance[sizeof(CookTorrance)];
} Rs_storage;
BSDF *Rs = NULL;
switch (specular_mode)
{
case 0:
Rs = new (Rs_storage.ward) Ward(F, shiny_u, shiny_v);
break;
case 1:
Rs = new (Rs_storage.phong) StretchedPhong(F, shiny_u);
break;
case 2:
Rs = new (Rs_storage.blinn) Blinn(F, shiny_u);
break;
case 3:
Rs = new (Rs_storage.cooktorrance) CookTorrance(F, 1.0f / shiny_u);
break;
}
SpecularReflection Rr(F);
scalar refr_shiny_u = refraction_glossiness;
if (refr_shiny_u < eiSCALAR_EPS)
{
refr_shiny_u = eiSCALAR_EPS;
refr_samples = 1;
}
refr_shiny_u = max(0.0f, glossiness_scale / refr_shiny_u);
scalar refr_shiny_v = max(0.0f, shiny_u * anisotropy);
union {
eiByte ward[sizeof(Ward)];
eiByte phong[sizeof(StretchedPhong)];
eiByte blinn[sizeof(Blinn)];
eiByte cooktorrance[sizeof(CookTorrance)];
} Rts_storage;
BSDF *Rts = NULL;
switch (specular_mode)
{
case 0:
Rts = new (Rts_storage.ward) Ward(invF, refr_shiny_u, refr_shiny_v);
break;
case 1:
Rts = new (Rts_storage.phong) StretchedPhong(invF, refr_shiny_u);
break;
case 2:
Rts = new (Rts_storage.blinn) Blinn(invF, refr_shiny_u);
break;
case 3:
Rts = new (Rts_storage.cooktorrance) CookTorrance(invF, 1.0f / refr_shiny_u);
break;
}
scalar refr_thickness = refraction_thickness;
// internal scale for refraction thickness, make it smaller
BRDFtoBTDF Rt(Rts, IOR, refr_thickness * 0.1f, this);
color Cdiffuse(0.0f);
color Cspecular(0.0f);
// don't integrate direct lighting if the ray hits the back face
if (dot_nd < 0.0f)
{
// integrate direct lighting from the front side
//out->Ci += integrate_direct_lighting(/*Kd*/diffuse(), Rd, wo);
//out->Ci *= i_diffuse() * getDiffuse(Nf, eiFALSE, eiFALSE);
Cdiffuse += i_diffuse() * getDiffuse(Nf, eiFALSE, eiFALSE);
//out->Ci += integrate_direct_lighting(Ks, *Rs, wo);
//out->Ci += i_specularColor() * getPhong (Nf, V, i_cosinePower(), eiFALSE, eiFALSE);
Cspecular += i_specularColor() * getPhong (Nf, V, i_cosinePower(), eiFALSE, eiFALSE);
}
// integrate for translucency from the back side
if (!almost_black( refraction_color * (specular_weight * refraction_weight * translucency_weight)*(is_metal?Cs:WHITE) ) && //almost_black(Kc)
(refr_thickness > 0.0f || dot_nd > 0.0f))
{
vector old_dPdu(dPdu);
vector old_dPdv(dPdv);
vector old_u_axis(u_axis);
vector old_v_axis(v_axis);
normal old_N(N);
vector new_wo(wo);
if (dot_nd < 0.0f)
{
dPdu = old_dPdv;
dPdv = old_dPdu;
u_axis = old_v_axis;
v_axis = old_u_axis;
N = -N;
new_wo = vector(wo.y, wo.x, -wo.z);
}
// integrate direct lighting from the back side
//out->Ci += Kc * integrate_direct_lighting(/*Kd*/i_diffuse(), Rd, new_wo);
//out->Ci += i_diffuse() * getDiffuse(Nf, eiFALSE, eiFALSE);
Cdiffuse += i_diffuse() * getDiffuse(Nf, eiFALSE, eiFALSE);
//out->Ci += Kc * integrate_direct_lighting(Ks, *Rs, new_wo);
//out->Ci += i_specularColor() * getPhong (Nf, V, i_cosinePower(), eiFALSE, eiFALSE);
Cspecular += i_specularColor() * getPhong (Nf, V, i_cosinePower(), eiFALSE, eiFALSE);
N = old_N;
u_axis = old_u_axis;
v_axis = old_v_axis;
dPdu = old_dPdu;
dPdv = old_dPdv;
}
scalar cutoff_thresh = cutoff_threshold;
color CReflectSpecular(0.0f);
// integrate indirect specular lighting
if (!almost_black( specular_color * (specular_weight * (1.0f - reflection_weight))*(is_metal?Cs:WHITE) ) && dot_nd < 0.0f)//almost_black(Ks)
{
IntegrateOptions opt;
opt.ray_type = EI_RAY_REFLECT_GLOSSY;
opt.min_samples = opt.max_samples = refl_samples;
opt.cutoff_threshold = cutoff_thresh;
CReflectSpecular = integrate(wo, *Rs, opt);
}
color CSpecularReflection(0.0f);
// integrate perfect specular reflection
if (!almost_black(reflection_color * (specular_weight * reflection_weight) * (is_metal?Cs:WHITE)) && dot_nd < 0.0f)//almost_black(Kr)
{
IntegrateOptions opt;
opt.ray_type = EI_RAY_REFLECT_GLOSSY;
opt.min_samples = opt.max_samples = 1; // force one sample for reflection
opt.cutoff_threshold = cutoff_thresh;
// the direct lighting of this BRDF is not accounted,
// so we trace lights here to compensate
opt.trace_lights = eiTRUE;
CSpecularReflection = integrate(wo, Rr, opt);
}
color CReflectDiffuse(0.0f);
// integrate indirect diffuse lighting (color bleeding)
if (!almost_black( Cs *(1.0f - specular_weight) * diffuse_weight ) && dot_nd < 0.0f)//almost_black(Kd)
{
IntegrateOptions opt;
opt.ray_type = EI_RAY_REFLECT_DIFFUSE;
opt.min_samples = opt.max_samples = diffuse_samples;
opt.cutoff_threshold = cutoff_thresh;
CReflectDiffuse = integrate(wo, Rd, opt);
}
color CRefraction(0.0f);
// integrate refraction
if ( !almost_black(refraction_color * specular_weight * refraction_weight * (1.0f-translucency_weight)*(is_metal?Cs:WHITE)) ) //almost_black(Kt)
{
IntegrateOptions opt;
opt.ray_type = EI_RAY_REFRACT_GLOSSY;
if (IOR == 1.0f)
{
opt.ray_type = EI_RAY_TRANSPARENT;
}
opt.min_samples = opt.max_samples = refr_samples;
opt.cutoff_threshold = cutoff_thresh;
// account for refractive caustics
opt.trace_lights = eiTRUE;
CRefraction = integrate(wo, Rt, opt);
}
out->Ci *= (Cdiffuse
+ CReflectDiffuse *
Cs *(1.0f - specular_weight) * diffuse_weight//Kd
);
out->Ci += (Cspecular
+ CReflectSpecular*
specular_color * specular_weight * (1.0f - reflection_weight)*(is_metal?Cs:WHITE)//Ks
+ CSpecularReflection*
reflection_color * specular_weight * reflection_weight * (is_metal?Cs:WHITE)//Kr
+ CRefraction*
refraction_color * specular_weight * refraction_weight * (1.0f-translucency_weight) * (is_metal?Cs:WHITE)//Kt
);
#ifdef USE_AOV_aov_ambient
aov_ambient() += ( i_ambientColor()
*(CReflectDiffuse *
Cs *(1.0f - specular_weight) * diffuse_weight//Kd
)
) * (1.0f - o_outTransparency());
#endif
#ifdef USE_AOV_aov_diffuse
aov_diffuse() += ( Cdiffuse * i_color() ) * (1.0f - o_outTransparency());
#endif
#ifdef USE_AOV_aov_specular
aov_specular() += (Cspecular
+ CReflectSpecular*
specular_color * specular_weight * (1.0f - reflection_weight)*(is_metal?Cs:WHITE)//Ks
+ CSpecularReflection*
reflection_color * specular_weight * reflection_weight * (is_metal?Cs:WHITE)//Kr
+ CRefraction*
refraction_color * specular_weight * refraction_weight * (1.0f-translucency_weight) * (is_metal?Cs:WHITE)//Kt
);
#endif
if ( ! less_than( &i_transparency(), LIQ_SCALAR_EPSILON ) )
{//transparent
out->Ci = out->Ci * ( 1.0f - i_transparency() ) + trace_transparent() * i_transparency();
}//else{ opacity }
Rs->~BSDF();
Rts->~BSDF();
}
//-----------------------------------------------------------------------------
// solve()
//-----------------------------------------------------------------------------
void LLJointSolverRP3::solve()
{
// llinfos << llendl;
// llinfos << "LLJointSolverRP3::solve()" << llendl;
//-------------------------------------------------------------------------
// setup joints in their base rotations
//-------------------------------------------------------------------------
mJointA->setRotation( mJointABaseRotation );
mJointB->setRotation( mJointBBaseRotation );
//-------------------------------------------------------------------------
// get joint positions in world space
//-------------------------------------------------------------------------
LLVector3 aPos = mJointA->getWorldPosition();
LLVector3 bPos = mJointB->getWorldPosition();
LLVector3 cPos = mJointC->getWorldPosition();
LLVector3 gPos = mJointGoal->getWorldPosition();
// llinfos << "bPosLocal = " << mJointB->getPosition() << llendl;
// llinfos << "cPosLocal = " << mJointC->getPosition() << llendl;
// llinfos << "bRotLocal = " << mJointB->getRotation() << llendl;
// llinfos << "cRotLocal = " << mJointC->getRotation() << llendl;
// llinfos << "aPos : " << aPos << llendl;
// llinfos << "bPos : " << bPos << llendl;
// llinfos << "cPos : " << cPos << llendl;
// llinfos << "gPos : " << gPos << llendl;
//-------------------------------------------------------------------------
// get the poleVector in world space
//-------------------------------------------------------------------------
LLMatrix4 worldJointAParentMat;
if ( mJointA->getParent() )
{
worldJointAParentMat = mJointA->getParent()->getWorldMatrix();
}
LLVector3 poleVec = rotate_vector( mPoleVector, worldJointAParentMat );
//-------------------------------------------------------------------------
// compute the following:
// vector from A to B
// vector from B to C
// vector from A to C
// vector from A to G (goal)
//-------------------------------------------------------------------------
LLVector3 abVec = bPos - aPos;
LLVector3 bcVec = cPos - bPos;
LLVector3 acVec = cPos - aPos;
LLVector3 agVec = gPos - aPos;
// llinfos << "abVec : " << abVec << llendl;
// llinfos << "bcVec : " << bcVec << llendl;
// llinfos << "acVec : " << acVec << llendl;
// llinfos << "agVec : " << agVec << llendl;
//-------------------------------------------------------------------------
// compute needed lengths of those vectors
//-------------------------------------------------------------------------
F32 abLen = abVec.magVec();
F32 bcLen = bcVec.magVec();
F32 agLen = agVec.magVec();
// llinfos << "abLen : " << abLen << llendl;
// llinfos << "bcLen : " << bcLen << llendl;
// llinfos << "agLen : " << agLen << llendl;
//-------------------------------------------------------------------------
// compute component vector of (A->B) orthogonal to (A->C)
//-------------------------------------------------------------------------
LLVector3 abacCompOrthoVec = abVec - acVec * ((abVec * acVec)/(acVec * acVec));
// llinfos << "abacCompOrthoVec : " << abacCompOrthoVec << llendl;
//-------------------------------------------------------------------------
// compute the normal of the original ABC plane (and store for later)
//-------------------------------------------------------------------------
LLVector3 abcNorm;
if (!mbUseBAxis)
{
if( are_parallel(abVec, bcVec, 0.001f) )
{
// the current solution is maxed out, so we use the axis that is
// orthogonal to both poleVec and A->B
if ( are_parallel(poleVec, abVec, 0.001f) )
{
// ACK! the problem is singular
if ( are_parallel(poleVec, agVec, 0.001f) )
{
// the solutions is also singular
return;
}
else
{
abcNorm = poleVec % agVec;
}
}
else
{
abcNorm = poleVec % abVec;
}
}
else
{
abcNorm = abVec % bcVec;
}
}
else
{
abcNorm = mBAxis * mJointB->getWorldRotation();
}
//-------------------------------------------------------------------------
// compute rotation of B
//-------------------------------------------------------------------------
// angle between A->B and B->C
F32 abbcAng = angle_between(abVec, bcVec);
// vector orthogonal to A->B and B->C
LLVector3 abbcOrthoVec = abVec % bcVec;
if (abbcOrthoVec.magVecSquared() < 0.001f)
{
abbcOrthoVec = poleVec % abVec;
abacCompOrthoVec = poleVec;
}
abbcOrthoVec.normVec();
F32 agLenSq = agLen * agLen;
// angle arm for extension
F32 cosTheta = (agLenSq - abLen*abLen - bcLen*bcLen) / (2.0f * abLen * bcLen);
if (cosTheta > 1.0f)
cosTheta = 1.0f;
else if (cosTheta < -1.0f)
cosTheta = -1.0f;
F32 theta = acos(cosTheta);
LLQuaternion bRot(theta - abbcAng, abbcOrthoVec);
// llinfos << "abbcAng : " << abbcAng << llendl;
// llinfos << "abbcOrthoVec : " << abbcOrthoVec << llendl;
// llinfos << "agLenSq : " << agLenSq << llendl;
// llinfos << "cosTheta : " << cosTheta << llendl;
// llinfos << "theta : " << theta << llendl;
// llinfos << "bRot : " << bRot << llendl;
// llinfos << "theta abbcAng theta-abbcAng: " << theta*180.0/F_PI << " " << abbcAng*180.0f/F_PI << " " << (theta - abbcAng)*180.0f/F_PI << llendl;
//-------------------------------------------------------------------------
// compute rotation that rotates new A->C to A->G
//-------------------------------------------------------------------------
// rotate B->C by bRot
bcVec = bcVec * bRot;
// update A->C
acVec = abVec + bcVec;
LLQuaternion cgRot;
cgRot.shortestArc( acVec, agVec );
// llinfos << "bcVec : " << bcVec << llendl;
// llinfos << "acVec : " << acVec << llendl;
// llinfos << "cgRot : " << cgRot << llendl;
// update A->B and B->C with rotation from C to G
abVec = abVec * cgRot;
bcVec = bcVec * cgRot;
abcNorm = abcNorm * cgRot;
acVec = abVec + bcVec;
//-------------------------------------------------------------------------
// compute the normal of the APG plane
//-------------------------------------------------------------------------
if (are_parallel(agVec, poleVec, 0.001f))
{
// the solution plane is undefined ==> we're done
return;
}
LLVector3 apgNorm = poleVec % agVec;
apgNorm.normVec();
if (!mbUseBAxis)
{
//---------------------------------------------------------------------
// compute the normal of the new ABC plane
// (only necessary if we're NOT using mBAxis)
//---------------------------------------------------------------------
if( are_parallel(abVec, bcVec, 0.001f) )
{
// G is either too close or too far away
// we'll use the old ABCnormal
}
else
{
abcNorm = abVec % bcVec;
}
abcNorm.normVec();
}
//-------------------------------------------------------------------------
// calcuate plane rotation
//-------------------------------------------------------------------------
LLQuaternion pRot;
if ( are_parallel( abcNorm, apgNorm, 0.001f) )
{
if (abcNorm * apgNorm < 0.0f)
{
// we must be PI radians off ==> rotate by PI around agVec
pRot.setQuat(F_PI, agVec);
}
else
{
// we're done
}
}
else
{
pRot.shortestArc( abcNorm, apgNorm );
}
// llinfos << "abcNorm = " << abcNorm << llendl;
// llinfos << "apgNorm = " << apgNorm << llendl;
// llinfos << "pRot = " << pRot << llendl;
//-------------------------------------------------------------------------
// compute twist rotation
//-------------------------------------------------------------------------
LLQuaternion twistRot( mTwist, agVec );
// llinfos << "twist : " << mTwist*180.0/F_PI << llendl;
// llinfos << "agNormVec: " << agNormVec << llendl;
// llinfos << "twistRot : " << twistRot << llendl;
//-------------------------------------------------------------------------
// compute rotation of A
//-------------------------------------------------------------------------
LLQuaternion aRot = cgRot * pRot * twistRot;
//-------------------------------------------------------------------------
// apply the rotations
//-------------------------------------------------------------------------
mJointB->setWorldRotation( mJointB->getWorldRotation() * bRot );
mJointA->setWorldRotation( mJointA->getWorldRotation() * aRot );
}
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 );
}
void pointer(struct robot *robot)
{
struct vect point;
point.x = (cosf(robot->rotational_target + (robot->rotational_velocity * 0)) * 1000) + robot->position2.position.x;
point.y = (sinf(robot->rotational_target + (robot->rotational_velocity * 0)) * 1000) + robot->position2.position.y;
point.z = -40;
//fprintf(stderr, "%f, %f, %f\n\n", robot->turret.target.x, robot->turret.target.y, robot->turret.target.z);
point_at_world(robot, &robot->turret.target);
struct vect cam_in_world;
local_to_world(robot, &cam_in_world, &camera_center);
// fprintf(stderr, "%f, %f, %f\n", cam_in_world.x, cam_in_world.y, cam_in_world.z);
// fprintf(stderr, "%f, %f\n", robot->position2.position.x, robot->position2.position.y);
struct vect d;
difference(&d, &cam_in_world, &point);
float xx = d.x * d.x;
float yy = d.y * d.y;
float zz = d.z * d.z;
float predicted_distance = sqrtf(xx + yy + zz) - 40;
//fprintf(stderr, "%f, %f\n", predicted_distance, robot->range);
if (predicted_distance < robot->range)
{
gpioWrite(14, 0);
}
else
{
gpioWrite(14, 0);
}
struct vect output;
get_up_vector(robot, &output);
// printf("%f, %f, %f\n", output.x, output.y, output.z);
struct vect cam_up, up, look, cam_forward, product;
up.x = 0;
up.y = 0;
up.z = 1.0;
cam_forward.x = 0;
cam_forward.y = -1;
cam_forward.z = 0;
rotate_vector(&cam_up, &up, &robot->turret.turret_rotation);
rotate_vector(&look, &cam_forward, &robot->turret.turret_rotation);
robot->turret.roll = acosf(dot(&cam_up, &output));
cross(&cam_up, &output, &product);
robot->turret.roll = -asinf(product.x / look.x);
if (robot->turret.pitch > robot->turret.pitch_guess)
{
robot->turret.pitch_guess += 0.000000174532925;
if (robot->turret.pitch_guess > robot->turret.pitch)
{
robot->turret.pitch_guess = robot->turret.pitch;
}
}
if (robot->turret.pitch < robot->turret.pitch_guess)
{
robot->turret.pitch_guess -= 0.000000174532925;
if (robot->turret.pitch_guess < robot->turret.pitch)
{
robot->turret.pitch_guess = robot->turret.pitch;
}
}
if (robot->turret.yaw > robot->turret.yaw_guess)
{
robot->turret.yaw_guess += 0.000000174532925;
if (robot->turret.yaw_guess > robot->turret.yaw)
{
robot->turret.yaw_guess = robot->turret.yaw;
}
}
if (robot->turret.yaw < robot->turret.yaw_guess)
{
robot->turret.yaw_guess -= 0.000000174532925;
if (robot->turret.yaw_guess < robot->turret.yaw)
{
robot->turret.yaw_guess = robot->turret.yaw;
}
}
//printf("%f, %f, %f\n", local.x, local.y, local.z);
//fprintf(stderr, "%f\n", robot->turret.roll);
}
