int
spawnplayer ( world_t *world )
{
int i;
entity_t *playerent,*e=NULL;
/* find a spawn point */
for(i=0;i<world->numentities;i++)
{
e = &world->entities[i];
if(e->type==ENTITYTYPE_SPAWN)
break;
}
if(i==world->numentities)
{
fprintf(stderr,"Could not find spawn point\n");
return 0;
}
world->playerentity = playerent = allocentity ( world );
vectorcopy(&playerent->pos, &e->pos);
vectorcopy(&playerent->angle, &e->angle);
playerent->type = ENTITYTYPE_PLAYER;
playerent->vvel = 0.0f;
playerent->onground = 1;
playerent->keys = 0;
playerent->physics = physics_player;
playerent->currentplatform = pickplatform ( world->raycaster,
&playerent->pos);
if(playerent->currentplatform == &world->raycaster->level.infplatform)
printf("Warning: Spawn point on infplat\n");
playerent->vpos = playerent->currentplatform->floorheight;
return 1;
}
void
setupworld ( world_t *world )
{
int i;
entity_t *e;
vector2d_t angle,dir,verts[2];
world->time = SDL_GetTicks();
if(!world->playerentity)
{
if(!spawnplayer( world ))
return;
}
for(i=0;i<world->numentities;i++)
{
e = &world->entities[i];
if(e->think && world->time > e->nextthink)
{
void (*think)(world_t *w, struct entity_s *e);
think = e->think;
e->think = NULL;
think(world,e);
}
/* don't need to add transparent stuff to the world */
if(!e->texture)
continue;
if(e->follow)
setfollowangle(world,e,&angle);
else
vectorcopy(&angle,&e->angle);
vectorrot90( &angle, &dir );
vectorscale( &dir, e->texture->width/2, &dir );
vectorsubtract( &e->pos, &dir, &verts[0]);
vectoradd( &e->pos, &dir, &verts[1]);
addsprite(world->raycaster,verts,e->texture->height,e->vpos,
SURFACE_NONE,e->texture);
}
/* copy over player view pos to raycaster */
world->raycaster->currentplatform = world->playerentity->currentplatform;
vectorcopy(&world->raycaster->viewpos,&world->playerentity->pos);
vectorcopy(&world->raycaster->viewdir,&world->playerentity->angle);
world->raycaster->eyelevel = world->playerentity->vpos+VIEW_HEIGHT;
}
void
setfollowangle ( world_t *w, entity_t *e, vector2d_t *normal )
{
/* vectorsubtract( &w->playerentity->pos, &e->pos, normal );
vectornormalise(normal,normal);*/
vectorcopy( normal, &w->playerentity->angle );
}
void ft_impact(t_draw_suite *val, t_ray *ray, t_tool *t)
{
val->impact->o = vectoradd(ray->o,
vectorscale(val->curobject->dist, ray->d));
find_normal(val->impact, val->curobject);
vectornorm(val->impact->d);
init_color(t, val->curobject->color, val->final_color);
val->curlight = t->l_lights;
while (val->curlight)
{
val->lightray->o = vectorcopy(val->curlight->o);
val->lightray->d = vectorsub(val->impact->o, val->lightray->o);
vectornorm(val->lightray->d);
if ((val->curobject2 = intersection(t->l_objects,
val->lightray)) && val->curobject2 == val->curobject)
ft_impact2(val);
val->curlight = val->curlight->next;
}
}
void imucalculateestimatedattitude(void)
{
float EstG[3];
float acc[3]; // holds float accel vector
float deltatime; // time in seconds
float gyros[3];
vectorcopy ( &EstG[0] , &GEstG[0] );
deltatime = (float)lib_timers_gettimermicrosecondsandreset(&gp_timer) * 0.000001f ; // time in seconds
deltatime = deltatime* 0.92; // correction factor
// unknown reason
readgyro();
readacc();
// correct the gyro and acc readings to remove error
// x & y accel offsets only
for ( int x = 0; x < 3; ++x) { // was 3
global.gyrorate[x] = global.gyrorate[x] + usersettings.gyrocalibration[x];
global.acc_g_vector[x] = global.acc_g_vector[x] + usersettings.acccalibration[x];
acc[x]= global.acc_g_vector[x]>>6;
}
// deadzone for yaw rate
//global.gyrorate[2] = ( 0 || global.gyrorate[2] > 40000 || global.gyrorate[2]< -40000 ) * global.gyrorate[2] ;
for ( int i = 0 ; i < 3; i++)
{
gyros[i] = tofloat( global.gyrorate[i] ) * deltatime * 0.01745329;
}
#ifndef SMALL_ANGLE_APPROX
// This does a "proper" matrix rotation using gyro deltas without small-angle approximation
float mat[3][3];
float tempvect[3];
float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
vectorcopy ( &tempvect[0] , & EstG[0] );
// the signs are differnt due to different conventions
// for positive/negative angles in various multiwii forks this is based on
cosx = _cosf( gyros[1]);
sinx = _sinf( gyros[1]);
cosy = _cosf( -gyros[0]);
siny = _sinf( -gyros[0]);
cosz = _cosf( -gyros[2]);
sinz = _sinf( -gyros[2]);
coszcosx = cosz * cosx;
coszcosy = cosz * cosy;
sinzcosx = sinz * cosx;
coszsinx = sinx * cosz;
sinzsinx = sinx * sinz;
mat[0][0] = coszcosy;
mat[0][1] = -cosy * sinz;
mat[0][2] = siny;
mat[1][0] = sinzcosx + (coszsinx * siny);
mat[1][1] = coszcosx - (sinzsinx * siny);
mat[1][2] = -sinx * cosy;
mat[2][0] = (sinzsinx) - (coszcosx * siny);
mat[2][1] = (coszsinx) + (sinzcosx * siny);
mat[2][2] = cosy * cosx;
EstG[0] = tempvect[0] * mat[0][0] + tempvect[1] * mat[1][0] + tempvect[2] * mat[2][0];
EstG[1] = tempvect[0] * mat[0][1] + tempvect[1] * mat[1][1] + tempvect[2] * mat[2][1];
EstG[2] = tempvect[0] * mat[0][2] + tempvect[1] * mat[1][2] + tempvect[2] * mat[2][2];
#endif // end rotation matrix
#ifdef SMALL_ANGLE_APPROX
// this is a rotation with small angle approximation
float deltagyroangle; // holds float gyro angle in rad
// Rotate Estimated vector(s), ROLL
EstG[2] = scos(gyros[0]) * EstG[2] - ssin(gyros[0]) * EstG[0];
EstG[0] = ssin(gyros[0]) * EstG[2] + scos(gyros[0]) * EstG[0];
// Rotate Estimated vector(s), PITCH
EstG[1] = scos(gyros[1]) * EstG[1] + ssin(gyros[1]) * EstG[2];
EstG[2] = -ssin(gyros[1]) * EstG[1] + scos(gyros[1]) * EstG[2];
// Rotate Estimated vector(s), YAW
EstG[0] = scos(gyros[2]) * EstG[0] - ssin(gyros[2]) * EstG[1];
EstG[1] = ssin(gyros[2]) * EstG[0] + scos(gyros[2]) * EstG[1];
#endif // end small angle approx
// yaw not tested ( maybe for yaw hold? )
// includes deadzone
// global.currentestimatedeulerattitude[YAWINDEX] += ( 0 || global.gyrorate[2] > 40000 || global.gyrorate[2]< -40000 ) * ( (float) ( global.gyrorate[2] ) * deltatime + 0.5f);
// yaw without deadzone
// not tested , i am not sure what the yaw angle is even used for
global.currentestimatedeulerattitude[YAWINDEX] += ( (float) ( global.gyrorate[2] ) * deltatime);
lib_fp_constrain180(&global.currentestimatedeulerattitude[YAWINDEX]);
// global.estimateddownvector[ZINDEX] < 0
// in pilotcontrol.c fix for inverted(not tested)
global.estimateddownvector[ZINDEX] = (EstG[2]>0)? 1111:-1111 ;
// orientation vector magnitude
float mag = 0;
mag = calcmagnitude( &EstG[0] );
// normalize orientation vector
if (1)
{
for (int axis = 0; axis < 3; axis++)
{
EstG[axis] = EstG[axis] / ( mag / ACC_1G );
}
}
//debug4 = mag;
// calc acc mag
float accmag;
accmag = calcmagnitude( &acc[0] );
// debug2 = accmag;
//normvector( acc , accmag, normal);
// normalize acc
for (int axis = 0; axis < 3; axis++)
{
acc[axis] = acc[axis] / (accmag / ACC_1G) ;
}
// test acc mag
//debug5 = calcmagnitude( &acc[0] );
/* Set the Gyro Weight for Gyro/Acc complementary filter */
/* Increasing this value would reduce and delay Acc influence on the output of the filter*/
// times for 3ms loop time
// filter time changes linearily with loop time
// 0.970 0.1s
// 0.988 0.25s
// 0.994 0.5 s
// 0.996 0.75 s
// 0.997 1.0 sec
// 0.998 1.5 sec
// 0.9985 2 sec
// 0.999 3 sec
// 0.99925 4 s
#define GYR_CMPF_FACTOR 0.998f // was 0.998
#define DISABLE_ACC 0
#define ACC_MIN 0.8f
#define ACC_MAX 1.2f
static unsigned int count = 0;
if ( ( accmag > ACC_MIN * ACC_1G ) && ( accmag < ACC_MAX * ACC_1G ) && !DISABLE_ACC )
{
//for (axis = 0; axis < 3; axis++)
if ( count >= 10 ) // loop time = 3ms so 30ms wait
{
//x4_set_leds( 0xFF);
EstG[0] = EstG[0] * GYR_CMPF_FACTOR + (float)acc[0] * ( 1.0f - GYR_CMPF_FACTOR );
EstG[1] = EstG[1] * GYR_CMPF_FACTOR + (float)acc[1] * ( 1.0f - GYR_CMPF_FACTOR );
EstG[2] = EstG[2] * GYR_CMPF_FACTOR + (float)acc[2] * ( 1.0f - GYR_CMPF_FACTOR );
}
count++;
}
else
{// acc mag out of bounds
//x4_set_leds( 0x00);
count = 0;
}
vectorcopy ( &GEstG[0] , &EstG[0]);
// convert our vectors to euler angles
global.currentestimatedeulerattitude[ROLLINDEX] = lib_fp_atan2(
FIXEDPOINTCONSTANT(EstG[0]*8 ),
FIXEDPOINTCONSTANT(EstG[2]*8 ) ) ;
/*
global.currentestimatedeulerattitude[PITCHINDEX] = lib_fp_atan2(
FIXEDPOINTCONSTANT( EstG[1]*8),
FIXEDPOINTCONSTANT( EstG[2]*8) );
*/
if (lib_fp_abs(global.currentestimatedeulerattitude[ROLLINDEX]) > FIXEDPOINT45 && lib_fp_abs(global.currentestimatedeulerattitude[ROLLINDEX]) < FIXEDPOINT135) {
global.currentestimatedeulerattitude[PITCHINDEX] =
lib_fp_atan2(EstG[1]*8, lib_fp_abs(EstG[0])*8);
} else {
global.currentestimatedeulerattitude[PITCHINDEX] =
lib_fp_atan2(
EstG[1]*8,
EstG[2]*8);
}
}
/* root of the monster's thinking - make a decision about
* what to do
*/
void
monster_think ( world_t *world, entity_t *ent )
{
vector2d_t dir;
float dist;
ent->wishdir.x = ent->wishdir.y = 0.0f;
ent->think = monster_think;
ent->nextthink = world->time + MONSTER_WIT;
if(!world->playerentity)
{
ent->texture = ent->frames[MONSTERFRAME_STAND];
ent->shoottime = -1;
return;
}
if(!pointcanseepoint(world->raycaster,
&world->playerentity->pos,world->playerentity->vpos+VIEW_HEIGHT,
&ent->pos,ent->vpos+MONSTER_MUZZLEHEIGHT))
{
ent->texture = ent->frames[MONSTERFRAME_STAND];
ent->nextthink = world->time + MONSTER_WIT*5; /* monster is not very alert in the absence of
the player */
ent->shoottime = -1;
return;
}
vectorsubtract(&world->playerentity->pos,&ent->pos,&dir);
dist = vectorlength(&dir);
vectorscale(&dir,1.0f/dist,&dir);
if(dist > MONSTER_SHOOTRANGE)
{
if((world->time/276)%2)
{
ent->texture = ent->frames[MONSTERFRAME_WALK1];
} else
{
ent->texture = ent->frames[MONSTERFRAME_WALK2];
}
vectorcopy(&ent->wishdir,&dir);
ent->shoottime = -1; /* if we back off and re-approach
* the monster still has to take aim
*/
return;
} else if(ent->shoottime > 0 && world->time > ent->shoottime )
{
ent->texture = ent->frames[MONSTERFRAME_FIRE];
ent->shoottime = world->time + MONSTER_RELOADTIME;
ent->think = monster_aimpose; /* return to aim pose */
ent->nextthink = world->time + MONSTER_FIRETIME;
return;
} else
{
if(ent->shoottime < 0)
{
ent->texture = ent->frames[MONSTERFRAME_AIM];
ent->shoottime = world->time + MONSTER_AIMTIME;
}
}
}
