1 // FGAIEntity - abstract base class an artificial intelligence entity
3 // Written by David Luff, started March 2002.
5 // Copyright (C) 2002 David C. Luff - david.luff@nottingham.ac.uk
7 // This program is free software; you can redistribute it and/or
8 // modify it under the terms of the GNU General Public License as
9 // published by the Free Software Foundation; either version 2 of the
10 // License, or (at your option) any later version.
12 // This program is distributed in the hope that it will be useful, but
13 // WITHOUT ANY WARRANTY; without even the implied warranty of
14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15 // General Public License for more details.
17 // You should have received a copy of the GNU General Public License
18 // along with this program; if not, write to the Free Software
19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
21 /*****************************************************************
23 * WARNING - Curt has some ideas about AI traffic so anything in here
24 * may get rewritten or scrapped. Contact Curt curt@flightgear.org
25 * before spending any time or effort on this code!!!
27 ******************************************************************/
29 #include <Main/globals.hxx>
30 #include <Scenery/scenery.hxx>
31 //#include <simgear/constants.h>
32 #include <simgear/math/point3d.hxx>
33 #include <simgear/math/sg_geodesy.hxx>
34 #include <simgear/misc/sg_path.hxx>
37 #include "AIEntity.hxx"
39 FGAIEntity::~FGAIEntity() {
42 void FGAIEntity::Update(double dt) {
45 // Run the internal calculations
46 //void FGAIEntity::Update() {
47 void FGAIEntity::Transform() {
48 aip.setPosition(pos.lon(), pos.lat(), pos.elev() * SG_METER_TO_FEET);
49 aip.setOrientation(roll, pitch, hdg);
54 void FGAIEntity::Transform() {
56 // Translate moving object w.r.t eye
57 Point3D sc = globals->get_scenery()->get_center();
58 //cout << "sc0 = " << sc.x() << " sc1 = " << sc.y() << " sc2 = " << sc.z() << '\n';
59 //cout << "op0 = " << obj_pos.x() << " op1 = " << obj_pos.y() << " op2 = " << obj_pos.z() << '\n';
62 FastWorldCoordinate(&shippos, sc);
63 position->setTransform( &shippos );
64 //cout << "Transform called\n";
69 // Taken from tileentry.cxx
70 void FGAIEntity::WorldCoordinate(sgCoord *obj_pos, Point3D center) {
72 Point3D geod( pos.lon() * SGD_DEGREES_TO_RADIANS,
73 pos.lat() * SGD_DEGREES_TO_RADIANS,
76 Point3D world_pos = sgGeodToCart( geod );
77 Point3D offset = world_pos - center;
80 sgMakeTransMat4( POS, offset.x(), offset.y(), offset.z() );
82 sgVec3 obj_rt, obj_up;
83 sgSetVec3( obj_rt, 0.0, 1.0, 0.0); // Y axis
84 sgSetVec3( obj_up, 0.0, 0.0, 1.0); // Z axis
86 sgMat4 ROT_lon, ROT_lat, ROT_hdg;
87 sgMakeRotMat4( ROT_lon, lon, obj_up );
88 sgMakeRotMat4( ROT_lat, 90 - lat, obj_rt );
89 sgMakeRotMat4( ROT_hdg, hdg, obj_up );
92 sgCopyMat4( TUX, ROT_hdg );
93 sgPostMultMat4( TUX, ROT_lat );
94 sgPostMultMat4( TUX, ROT_lon );
95 sgPostMultMat4( TUX, POS );
97 sgSetCoord( obj_pos, TUX );
101 // Norman's 'fast hack' for above
102 void FGAIEntity::FastWorldCoordinate(sgCoord *obj_pos, Point3D center) {
103 double lon_rad = pos.lon() * SGD_DEGREES_TO_RADIANS;
104 double lat_rad = pos.lat() * SGD_DEGREES_TO_RADIANS;
105 double hdg_rad = hdg * SGD_DEGREES_TO_RADIANS;
108 Point3D geod( lon_rad, lat_rad, pos.elev() );
110 Point3D world_pos = sgGeodToCart( geod );
111 Point3D offset = world_pos - center;
115 SGfloat sin_lat = (SGfloat)sin( lat_rad );
116 SGfloat cos_lat = (SGfloat)cos( lat_rad );
117 SGfloat cos_lon = (SGfloat)cos( lon_rad );
118 SGfloat sin_lon = (SGfloat)sin( lon_rad );
119 SGfloat sin_hdg = (SGfloat)sin( hdg_rad ) ;
120 SGfloat cos_hdg = (SGfloat)cos( hdg_rad ) ;
122 mat[0][0] = cos_hdg * (SGfloat)sin_lat * (SGfloat)cos_lon - sin_hdg * (SGfloat)sin_lon;
123 mat[0][1] = cos_hdg * (SGfloat)sin_lat * (SGfloat)sin_lon + sin_hdg * (SGfloat)cos_lon;
124 mat[0][2] = -cos_hdg * (SGfloat)cos_lat;
127 mat[1][0] = -sin_hdg * (SGfloat)sin_lat * (SGfloat)cos_lon - cos_hdg * (SGfloat)sin_lon;
128 mat[1][1] = -sin_hdg * (SGfloat)sin_lat * (SGfloat)sin_lon + cos_hdg * (SGfloat)cos_lon;
129 mat[1][2] = sin_hdg * (SGfloat)cos_lat;
132 mat[2][0] = (SGfloat)cos_lat * (SGfloat)cos_lon;
133 mat[2][1] = (SGfloat)cos_lat * (SGfloat)sin_lon;
134 mat[2][2] = (SGfloat)sin_lat;
137 mat[3][0] = offset.x();
138 mat[3][1] = offset.y();
139 mat[3][2] = offset.z();
142 sgSetCoord( obj_pos, mat );