not working properly when targeted.

vec_t	V_LOCAL
qmotion_arccos(vec_t cosine)
{
	double sine;
	if(IsZero(cosine))
		return M_PI2;
	sine=sqrt(1-cosine*cosine);
	return (vec_t)atan(sine/cosine);
}


/*	Sync function: Quake-like motion.
 *	Changes state of keymap, control and camera.
 */
/*
 *	Note: right vector of camera MUST lie in the base plane,
 *	(currently, third coordinate of 'right' must be zero);
 *	otherwise unpredictable results can occur.
 *	TODO: add additional fields into cam_t.
 */
void	V_LOCAL
sdcQSyncronize(sdof_keymap_t *km,sdof_control_t *sdc,cam_t *cam)
{
	int i;
	vec_t curtime;
	char zero_accel[3]={1,1,1}, zero_angle_accel[3]={1,1,1};
	vec3_t accel={0,0,0}, angle_accel={0,0,0}, right, dp, dmt, drt;
	vec3_t new_origin, prev_speed, prev_angle_speed;
	vec3_t base_dir;
	vec_t pitch,pitch_dot,max_pitch_dot=(vec_t)cos(5 * ANGLE_TO_RAD);
	V_ASSERT(km && sdc && cam);
	curtime=(vec_t)0.001*timerMilliseconds(&km->timer);
	/*
	 *	Stepping.
	 */
	/* TODO: implement smooth stepping */
	km->stepping_forward=false;
	km->stepping_backward=false;
	km->stepping_left=false;
	km->stepping_right=false;
	km->stepping_up=false;
	km->stepping_down=false;
	/* Calc accels */
	if(km->moving_right)
		accel[0]-=sdc->accel[0];
	if(km->moving_left)
		accel[0]+=sdc->accel[0];
	if(!IsZero(accel[0]))
		zero_accel[0]=0;
	if(km->moving_forward)
		accel[1]-=sdc->accel[1];
	if(km->moving_backward)
		accel[1]+=sdc->accel[1];
	if(!IsZero(accel[1]))
		zero_accel[1]=0;
	if(km->moving_down)
		accel[2]-=sdc->accel[2];
	if(km->moving_up)
		accel[2]+=sdc->accel[2];
	if(!IsZero(accel[2]))
		zero_accel[2]=0;
	if(km->pitching_up)
		angle_accel[0]-=sdc->angle_accel[0];
	if(km->pitching_down)
		angle_accel[0]+=sdc->angle_accel[0];
	if(!IsZero(angle_accel[0]))
		zero_angle_accel[0]=0;
	if(km->yawing_right)
		angle_accel[1]-=sdc->angle_accel[1];
	if(km->yawing_left)
		angle_accel[1]+=sdc->angle_accel[1];
	if(!IsZero(angle_accel[1]))
		zero_angle_accel[1]=0;
	/* compute speed deltas, store in accel vector */
	for(i=0;;i++)
	{
		dmt[i]=curtime-km->moving_times[i];
		drt[i]=curtime-km->rotation_times[i];
		if(zero_accel[i])
		{
			if(v_abs(sdc->speed[i])>sdc->zero_speed[i])
				if(sdc->speed[i]>0)
					accel[i]=-sdc->fade_accel[i]*dmt[i];
				else
					accel[i]=sdc->fade_accel[i]*dmt[i];
			else
				sdc->speed[i]=0;
		}
		else
			accel[i]*=dmt[i];
		if(i==2)
			break;
		if(zero_angle_accel[i])
		{
			if(v_abs(sdc->angle_speed[i])>sdc->zero_angle_speed[i])
				if(sdc->angle_speed[i]>0)
					angle_accel[i]=-sdc->fade_angle_accel[i]*drt[i];
				else
					angle_accel[i]=sdc->fade_angle_accel[i]*drt[i];
			else
				sdc->angle_speed[i]=0;
		}
		else
			angle_accel[i]*=drt[i];
	}
	/* Calc sdc speeds and clamp it to [-max .. max] */
	vec3Copy(prev_speed,sdc->speed);
	vec3Copy(prev_angle_speed,sdc->angle_speed);
	vec3Add(sdc->speed,sdc->speed,accel);
	vec3Add(sdc->angle_speed,sdc->angle_speed,angle_accel);
	for(i=0;;i++)
	{
		/* Prevent oscillation by checking speed sign */
		if(zero_accel[i])
		{
			if(prev_speed[i]*sdc->speed[i]<0)
				sdc->speed[i]=0;
		}
		if(sdc->speed[i]>sdc->max_speed[i])
    		sdc->speed[i]=sdc->max_speed[i];
    	else
    		if(sdc->speed[i]<-sdc->max_speed[i])
    			sdc->speed[i]=-sdc->max_speed[i];
		if(i==2)
			break;
		if(zero_angle_accel[i])
		{
			if(prev_angle_speed[i]*sdc->angle_speed[i]<0)
				sdc->angle_speed[i]=0;
		}
		if(sdc->angle_speed[i]>sdc->max_angle_speed[i])
			sdc->angle_speed[i]=sdc->max_angle_speed[i];
		else
			if(sdc->angle_speed[i]<-sdc->max_angle_speed[i])
				sdc->angle_speed[i]=-sdc->max_angle_speed[i];
	}
	/*	motion: r-l, f-b, d-u */
	vec3Copy(new_origin,cam->origin);
	vec3ProjectOnPlane(base_dir,cam->dir,&cam->baseplane);
	if(!IsZero(sdc->speed[0]))
	{
		vec3Cross(right,cam->dir,cam->up);
		vec3ProjectOnPlane(right,right,&cam->baseplane);
		vec3Scale(dp,right,sdc->speed[0]*dmt[0]);
		vec3Sub(new_origin,new_origin,dp);
	}
	if(!IsZero(sdc->speed[1]))
	{
		vec3Scale(dp,base_dir,sdc->speed[1]*dmt[1]);
		vec3Sub(new_origin,new_origin,dp);
	}
	if(!IsZero(sdc->speed[2]))
	{
		vec3Scale(dp,cam->baseplane.normal,sdc->speed[2]*dmt[2]);
		vec3Add(new_origin,new_origin,dp);
	}
	camChangeOrigin(cam,new_origin);
	/* rotation: u-d, r-l, cw-ccw */
	/* mouse motion directly affects on angles */
	if(!IsZero(sdc->angle_speed[0]) || !IsZero(km->angle_axis[0]))
	{
		vec3Cross(right,cam->dir,cam->up);
		pitch=km->angle_axis[0]+sdc->angle_speed[0]*drt[0];
		vec3RotateAround(dp,right,cam->dir,pitch);
		pitch_dot=vec3Dot(cam->baseplane.normal,dp);
		if(pitch>0)
		{
    		/* pitching up: upper limit is max_pitch_dot */
    		if(pitch_dot>max_pitch_dot)
    		{
	   			vec3RotateAround(dp,right,cam->baseplane.normal,
	   				-qmotion_arccos(max_pitch_dot));
	   			sdc->angle_speed[0]=0;
	   		}
    		vec3Copy(cam->dir,dp);
		}
		else
		{
    		/* pitching down: lower limit is -max_pitch_dot */
    		if(pitch_dot<-max_pitch_dot)
    		{
				vec3RotateAround(dp,right,cam->baseplane.normal,
					M_PI-qmotion_arccos(-max_pitch_dot));
	   			sdc->angle_speed[0]=0;
	   		}
    		vec3Copy(cam->dir,dp);
		}
		/* regenerate up vector */
		vec3Cross(cam->up,right,cam->dir);
	}
	if(!IsZero(sdc->angle_speed[1]) || !IsZero(km->angle_axis[1]))
	{
		matrix3_t zrot;
		mat3Rotate(zrot,cam->baseplane.normal,
			km->angle_axis[1]+sdc->angle_speed[1]*drt[1]);
		mat3VecMul(dp,zrot,cam->up);
		vec3Copy(cam->up,dp);
		mat3VecMul(dp,zrot,cam->dir);
		vec3Copy(cam->dir,dp);
	}
	if(cam->targeted)
		camTargetObject(cam,cam->target);
	/* update last access time in keymap */
	vec3Zero(km->angle_axis);
	vec3Set(km->moving_times,curtime,curtime,curtime);
	vec3Set(km->rotation_times,curtime,curtime,curtime);
	cam->modified=true;
}

