[Gcblue-commits] gcb_wx/src/sim tcTorpedoObject.cpp,NONE,1.1

[Dewitt C. <ddc...@us...>]

Update of /cvsroot/gcblue/gcb_wx/src/sim
In directory sc8-pr-cvs1.sourceforge.net:/tmp/cvs-serv22179/src/sim

Added Files:
	tcTorpedoObject.cpp 
Log Message:
Sonar work, passive sonar, torpedoes

--- NEW FILE: tcTorpedoObject.cpp ---
/**
**  @file tcTorpedoObject.cpp
*/
/*
**  Copyright (C) 2004 Dewitt Colclough (de...@tw...)
**  All rights reserved.
**
**  This file is part of the Global Conflict Blue (GCB) program.
**  GCB is free software; you can redistribute it and/or modify
**  it under the terms of version 2 of the GNU General Public License as 
**  published by the Free Software Foundation.
**
**  GCB is distributed in the hope that it will be useful,
**  but WITHOUT ANY WARRANTY; without even the implied warranty of
**  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
**  GNU General Public License for more details.
**
**  You should have received a copy of the GNU General Public License
**  along with GCB; if not, write to the Free Software
**  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
*/

#include "stdwx.h"
#include "tcTorpedoObject.h"
#include "common/tcStream.h"
#include "common/tcObjStream.h"
#include "database/tcGenericDBObject.h"
#include "database/tcTorpedoDBObject.h"
#include "tc3DModel.h"
#include "tcParticleEffect.h"
#include "tcLauncher.h"
#include "tcSimState.h"
#include "tc3DPoint.h"


#ifdef _DEBUG
#define new DEBUG_NEW
#endif

/**
* Load state from update stream
*/
tcUpdateStream& tcTorpedoObject::operator<<(tcUpdateStream& stream)
{
	tcWeaponObject::operator<<(stream);

	stream >> goalHeading_rad;
	stream >> goalPitch_rad;
	stream >> goalSpeed_kts; 
	stream >> interceptTime;
	stream >> lastGuidanceUpdate;
	stream >> guidanceUpdateInterval;
	waypoint << stream;
	stream >> rangeToObjective_km;
	stream >> currentSegment;
	seeker << stream;

	return stream;
}

/**
* Save state to update stream
*/
tcUpdateStream& tcTorpedoObject::operator>>(tcUpdateStream& stream)
{
	tcWeaponObject::operator>>(stream);

	stream << goalHeading_rad;
	stream << goalPitch_rad;
	stream << goalSpeed_kts; 
	stream << interceptTime;
	stream << lastGuidanceUpdate;
	stream << guidanceUpdateInterval;
	waypoint >> stream;
	stream << rangeToObjective_km;
	stream << currentSegment;
	seeker << stream;

	return stream;
}



/**
* @return guidance mode for current flight profile segment
* Use default profile for now
*/
int tcTorpedoObject::GetCurrentGuidanceMode()
{
    tcTorpedoDBObject::tsTorpedoSegment* segmentInfo = 
		&mpDBObject->defaultProfile[currentSegment];

	return (int)segmentInfo->meGuidanceMode;
}


/**
* Initializes missile state for launch from game object.
* Adds self to simulation
*
* @param obj launching game object
* @param launcher index of launcher
*/
void tcTorpedoObject::LaunchFrom(tcGameObject* obj, unsigned nLauncher)
{
    if (tcPlatformObject* platObj = dynamic_cast<tcPlatformObject*>(obj))
	{
		tc3DPoint launcherPos = platObj->mpDBObject->GetLauncherPosition(nLauncher);
		tsGeoPoint pos = obj->RelPosToLatLonAlt(launcherPos.x, launcherPos.y,
			launcherPos.z);
		mcKin.mfLon_rad = pos.mfLon_rad;
		mcKin.mfLat_rad = pos.mfLat_rad;
		mcKin.mfAlt_m = pos.mfAlt_m;
	}
	else
	{
		mcKin.mfLon_rad = obj->mcKin.mfLon_rad;
		mcKin.mfLat_rad = obj->mcKin.mfLat_rad;
		mcKin.mfAlt_m = obj->mcKin.mfAlt_m;
	}


    if (mcKin.mfAlt_m < 0)
    {
        mcKin.mfSpeed_kts = obj->mcKin.mfSpeed_kts + C_MPSTOKTS * mpDBObject->launchSpeed_mps;
        searchMode = SEARCH_S;
    }
    else // air launched
    {
        mcKin.mfSpeed_kts = obj->mcKin.mfSpeed_kts;
        searchMode = SEARCH_LEFTCIRCLE;
    }

	mcKin.mfHeading_rad = obj->mcKin.mfHeading_rad;
	mcKin.mfPitch_rad = obj->mcKin.mfPitch_rad;

	const tcLauncher* pLauncher = obj->GetLauncher(nLauncher);

	goalHeading_rad = obj->mcKin.mfHeading_rad + pLauncher->pointingAngle;


	waypoint = pLauncher->msDatum;
	
	mfStatusTime = obj->mfStatusTime;
	mcKin.mfHeading_rad += pLauncher->pointingAngle;
	mcKin.mfPitch_rad += pLauncher->pointingElevation;
	mcKin.mfClimbAngle_rad = mcKin.mfPitch_rad;

    fuel_kg = mpDBObject->fuel_kg; // start with max fuel

	tcString s;
	s.Format("From %s", obj->mzUnit.mz);
	mzUnit = s.GetBuffer();            
	mnAlliance = obj->mnAlliance;     

	simState->AddPlatform(static_cast<tcGameObject*>(this));

	// Set intended target (has to be done after alliance and id is set).
	// This is a tcWeaponObject method
	SetIntendedTarget(pLauncher->mnTargetID);

}

/**
* @return time remaining in seconds based on current fuel consumption
*/
float tcTorpedoObject::RuntimeRemaining()
{
    return 0; // not implemented yet
}
/**
*
*/
void tcTorpedoObject::SetHeading(float newHeading) 
{
    goalHeading_rad = newHeading;
}

/**
*
*/
void tcTorpedoObject::SetSpeed(float newSpeed) 
{
    goalSpeed_kts = newSpeed;
}

/**
*
*/
tcSonar* tcTorpedoObject::GetSensorState() 
{
    return &seeker;
}


/**
*
*/
void tcTorpedoObject::Update(double afStatusTime)
{
	float dt_s = (float)(afStatusTime - mfStatusTime);
	mfStatusTime = afStatusTime;

    wxASSERT(mpDBObject);

    // air launched torpedoes drop into water first
    bool outOfWater = (mcKin.mfAlt_m > 0.0f);
    if (outOfWater)
    {
        UpdateDrop(dt_s);
        return;
    }


    UpdateSpeedSimple(dt_s);

    // from tcPlatformObject Move method
    float heading_rad = mcKin.mfHeading_rad;
    float fGroundSpeed_kts = cosf(mcKin.mfClimbAngle_rad)*mcKin.mfSpeed_kts;
    double fDistance_rad = fGroundSpeed_kts*dt_s*(float)C_KTSTORADPS;

    mcKin.mfLon_rad += fDistance_rad*(double)(sinf(heading_rad)/cosf((float)mcKin.mfLat_rad));
    mcKin.mfLat_rad += (double)cosf(heading_rad)*fDistance_rad; 
    mcKin.mfAlt_m += sinf(mcKin.mfClimbAngle_rad)*mcKin.mfSpeed_kts*C_KTSTOMPS*dt_s;

	

	if (!clientMode)
	{
		UpdateGuidance(afStatusTime);
	}

	/*** heading calculation ***/
	float dh_rad, dh_min, dh_max;
	dh_rad = goalHeading_rad - mcKin.mfHeading_rad;
	radtoplusminuspi(dh_rad); // map dh_deg to [-pi,pi)
	dh_max = mpDBObject->maxTurnRate_radps * dt_s; 
	dh_min = -dh_max;
	if (dh_rad < dh_min) {dh_rad = dh_min;} // restrict to turn rate
	else if (dh_rad > dh_max) {dh_rad = dh_max;}
	mcKin.mfHeading_rad += dh_rad;
	if (mcKin.mfHeading_rad >= C_TWOPI) {mcKin.mfHeading_rad -= C_TWOPI;}
	if (mcKin.mfHeading_rad < 0) {mcKin.mfHeading_rad += C_TWOPI;}

	/*** pitch calculation ***/
	float dp_rad, dp_min, dp_max;
	dp_rad = goalPitch_rad - mcKin.mfPitch_rad;
	dp_max = 2 * mpDBObject->maxTurnRate_radps * dt_s;
	dp_min = -dp_max;
	if (dp_rad < dp_min) {dp_rad = dp_min;} 
	else if (dp_rad > dp_max) {dp_rad = dp_max;}
	mcKin.mfPitch_rad += dp_rad;
    mcKin.mfClimbAngle_rad = mcKin.mfPitch_rad;



	UpdateEffects();

    runTime += dt_s;

	if (clientMode) return;

	/*** check for crash ***/

	if (mcTerrain.mfHeight_m >= mcKin.mfAlt_m)
    {
		mfDamageLevel = 1.0f;
		tcString s;
		s.Format("Object %s hit bottom at time %.1f lon %.3f, lat %.3f",
			mzUnit.mz,afStatusTime,mcKin.mfLon_rad*C_180OVERPI,mcKin.mfLat_rad*C_180OVERPI);
		WTL(s.GetBuffer());
	}

    // update fuel
    fuel_kg -= dt_s * mpDBObject->fuelRate_kgpktps * mcKin.mfSpeed_kts;

	if (fuel_kg <= 0) 
    {
		mfDamageLevel = 1.0f; // self-destruct
		tcString s;
		s.Format("Object %s shut down at time %.1f lon %.3f, lat %.3f",
			mzUnit.mz,afStatusTime,mcKin.mfLon_rad*C_180OVERPI,mcKin.mfLat_rad*C_180OVERPI);
		WTL(s.GetBuffer());
	}

	seeker.Update(afStatusTime);

}

/**
*
*/
void  tcTorpedoObject::UpdateDrop(float dt_s)
{
    float heading_rad = mcKin.mfHeading_rad;
    float fGroundSpeed_kts = cosf(mcKin.mfClimbAngle_rad) * mcKin.mfSpeed_kts;
    float vz_mps = C_KTSTOMPS * sinf(mcKin.mfClimbAngle_rad) * mcKin.mfSpeed_kts;
    float vxy_mps = C_KTSTOMPS * fGroundSpeed_kts;
    float z = mcKin.mfAlt_m;

    float dvz = C_G * dt_s;

    if (vz_mps < -50)
    {
        dvz = 0;
    }
    else if (vz_mps < -40)
    {
        dvz = 0.1f * (vz_mps + 50) * dvz; // gradually limit acceleration
    }

    vz_mps = vz_mps - dvz;


    if (vxy_mps > 0)
    {
        vxy_mps = vxy_mps - dt_s * 0.02f * vxy_mps * vxy_mps; // air drag
    }
    if (vxy_mps < 0)
    {
        vxy_mps = 0;
    }

    double fDistance_rad = fGroundSpeed_kts*dt_s*(float)C_KTSTORADPS;

    mcKin.mfLon_rad += fDistance_rad*(double)(sinf(heading_rad)/cosf((float)mcKin.mfLat_rad));
    mcKin.mfLat_rad += (double)cosf(heading_rad)*fDistance_rad; 
    mcKin.mfAlt_m += vz_mps*dt_s;

    mcKin.mfClimbAngle_rad = atan2(vz_mps, vxy_mps);
    mcKin.mfPitch_rad = mcKin.mfClimbAngle_rad;
    mcKin.mfSpeed_kts = C_MPSTOKTS * sqrtf(vz_mps*vz_mps + vxy_mps*vxy_mps);
    
    if (mcKin.mfAlt_m <= 0) mcKin.mfSpeed_kts *= 0.1f; // lose speed after hitting water

	
}

/**
*
*/
void tcTorpedoObject::UpdateEffects()
{	
    if (model)
    {
        if ((mcKin.mfAlt_m < -5) && (mcKin.mfSpeed_kts > 15))
        {
            model->SetSmokeMode(tcParticleEffect::BUBBLES);
        }
        else
        {
            model->SetSmokeMode(tcParticleEffect::OFF);
        }

        model->UpdateEffects();
    }
}




/**
*
*/
void tcTorpedoObject::UpdateGuidance(double t) 
{
    using tcTorpedoDBObject::tsTorpedoSegment;

    const float torpedo_acz = 50.0f;
    const float one_over_torpedo_acz = 1.0f / 50.0f;

	if ((t - lastGuidanceUpdate) < guidanceUpdateInterval) return;
	lastGuidanceUpdate = t;

    unsigned int nSegments = mpDBObject->defaultProfile.size();
    wxASSERT(nSegments);
    
	if (currentSegment >= nSegments) return; // error case, shouldn't happen


	tsTorpedoSegment* segmentInfo = &mpDBObject->defaultProfile[currentSegment];
	float fGoalAltitude_m = mcKin.mfAlt_m;

    goalSpeed_kts = segmentInfo->thrust * mpDBObject->maxSpeed_kts;

	//************ GuidanceMode update **************
	bool useInterceptPitch = false;
	float interceptPitch_rad = 0;

	switch (segmentInfo->meGuidanceMode) 
	{
    case tcTorpedoDBObject::GM_NAV:
		goalHeading_rad = mcKin.HeadingToGeoRad(&waypoint);
		rangeToObjective_km = mcKin.RangeToKm(&waypoint);
        goalDepth_m = segmentInfo->depth_m;
		break;
    case tcTorpedoDBObject::GM_COMMAND:
        break;
	case tcTorpedoDBObject::GM_SENSOR:
        if (!seeker.GetActive()) 
		{
			seeker.SetActive(true);
	        seeker.mnMode = SSMODE_SEEKERSEARCH;
            searchHeading_rad = goalHeading_rad;
        }

        guidanceUpdateInterval = 0.5f;


		if (seeker.mnMode == SSMODE_SEEKERTRACK) 
		{
            if (seeker.mcTrack.mnFlags & TRACK_BEARING_ONLY)
            {
                goalHeading_rad = seeker.mcTrack.mfHeading_rad; // bearing in this case
                goalDepth_m = -seeker.mcTrack.mfAlt_m;
            }
            else
            {
                tcTrack predictedtrack;   
                float tti_s;
                seeker.mcTrack.GetPrediction(predictedtrack, t);

                mcKin.GetInterceptData3D(predictedtrack, goalHeading_rad, 
                    interceptPitch_rad, tti_s, rangeToObjective_km);

                rangeToObjective_km *= C_RADTOKM; // convert to km

                useInterceptPitch = true;
                interceptTime = tti_s;
            }
        }
        else if (seeker.mnMode == SSMODE_SEEKERSEARCH)
        {
            switch (searchMode)
            {
            case SEARCH_S:
                goalHeading_rad = searchHeading_rad + 0.4f * sinf(0.2f * runTime);
                break;
            case SEARCH_LEFTCIRCLE:
                goalHeading_rad = searchHeading_rad - 0.2f * runTime;
                goalHeading_rad = fmodf(goalHeading_rad, C_TWOPI);
                break;
            case SEARCH_RIGHTCIRCLE:
                goalHeading_rad = searchHeading_rad + 0.2f * runTime;
                goalHeading_rad = fmodf(goalHeading_rad, C_TWOPI);
                break;
            default:
                break;
            }
        }
        else if (seeker.mnMode == SSMODE_SEEKERACQUIRE)
        {
            goalHeading_rad = mcKin.mfHeading_rad; // maintain heading during acquire
            searchMode = SEARCH_LEFTCIRCLE; // to revert to circle search if acq fails
        }
        
        break;
	default:
        {
        fprintf(stderr, 
            "Error - tcTorpedoObject::UpdateGuidance - Bad segment guidance mode\n");
        }
		break;
	}

	// update guidance update rate and seeker scan rate based on time to intercept
	if (interceptTime <= 5.0) 
	{
		guidanceUpdateInterval = 0.2f;
		seeker.mfCurrentScanPeriod_s = 0.2f;
	}
	else if (interceptTime <= 2.0) 
	{
		guidanceUpdateInterval = 0.1f;
		seeker.mfCurrentScanPeriod_s = 0.1f;
	}


	//************ update altitude control **************


	if (useInterceptPitch) 
	{
        wxASSERT(!_isnan(interceptPitch_rad));
		goalPitch_rad = interceptPitch_rad;
	}
	else 
	{
        float depth_m = -mcKin.mfAlt_m;
		if (depth_m > goalDepth_m + torpedo_acz) 
		{
			goalPitch_rad = 0.7f;
		}
		else if (depth_m < goalDepth_m - torpedo_acz) 
		{
			goalPitch_rad = -0.7f;
		}
		else 
		{
            goalPitch_rad = 0.7f * (depth_m - goalDepth_m) * one_over_torpedo_acz;
		}
	}


	// swim level for first few seconds
	if (runTime < 5.0f)
	{
		goalHeading_rad = mcKin.mfHeading_rad;
		goalPitch_rad = 0.0f;
	}

	// switch to next segment if within range
	if (currentSegment < (nSegments-1)) 
	{
		if (rangeToObjective_km < segmentInfo->range_km) 
		{
			currentSegment++;
		}
	}
}

/**
* Simple model for speed update
* Flat max acceleration
*/
void tcTorpedoObject::UpdateSpeedSimple(float dt_s)
{
    float ds_kts = goalSpeed_kts - mcKin.mfSpeed_kts;
    float ds_max = mpDBObject->acceleration_ktsps * dt_s;
    float ds_min = -ds_max;

    if (ds_kts < ds_min) {ds_kts = ds_min;} // restrict to acceleration
    else if (ds_kts > ds_max) {ds_kts = ds_max;}

    mcKin.mfSpeed_kts += ds_kts;

	if (mcKin.mfSpeed_kts < 0) mcKin.mfSpeed_kts = 0;
}

/**
*
*/
void tcTorpedoObject::Clear()  
{  
	tcGameObject::Clear();
}

/**
*
*/
void tcTorpedoObject::DesignateTarget(long anID) 
{
	seeker.mcTrack.mnID = anID;

    // needs to SSMODE_SEEKERACQUIRE so track is updated before guidance update
	seeker.mnMode = SSMODE_SEEKERACQUIRE; 
}

/**
*
*/
void tcTorpedoObject::RandInitNear(float afLon, float afLat) 
{

	//strcpy(mzClass.mz,mpDBObject->mzClass.mz);
	mzUnit.AssignRandomStringB();
	mnAlliance = 0;
	mfStatusTime = 0;        
	mcKin.mfLon_rad = afLon + randfc(1.1f);      
	mcKin.mfLat_rad = afLat + randfc(1.1f);
          
	mcKin.mfHeading_rad = 360*randf();           
	mcKin.mfSpeed_kts = 100;
	mfDamageLevel = 0;    

	//tcGameObject::RandInitNear(afLon,afLat);
	mcKin.mfAlt_m = -100.0f;  
	SetHeading(mcKin.mfHeading_rad);
	SetSpeed(mcKin.mfSpeed_kts);    
	
	mcKin.SetRelativeGeo(waypoint, mcKin.mfHeading_rad*(float)C_PIOVER180, 100.0f);
}

/**
* If missile is targeting a land datum, gp.mnTargetID will be
* set to -1. Otherwise gp.mnTargetID is set to the intended 
* target of the missile.
*
* @return 1 if missile is in terminal phase
*/
int tcTorpedoObject::GetGuidanceParameters(tsGuidanceParameters& gp) 
{
	if (seeker.mnMode == SSMODE_SEEKERTRACK) 
	{
		gp.mnTargetID = seeker.mcTrack.mnID;

        if (seeker.IsPassive())
        {
            gp.mfInterceptTime = 0;
        }
        else
        {
		    gp.mfInterceptTime = interceptTime;
        }
		return 1;
	}
	else
	{
		return 0;
	}
}

/**
*
*/
void tcTorpedoObject::PrintToFile(tcFile& file)
{
	tcString s;
	tcGameObject::PrintToFile(file);
	s.Format(" Missile Object\n");
	file.WriteString(s.GetBuffer());
}

/**
*
*/
void tcTorpedoObject::SaveToFile(tcFile& file) 
{
	tcGameObject::SaveToFile(file);

    wxASSERT(false);
}

/**
*
*/
void tcTorpedoObject::LoadFromFile(tcFile& file) 
{
	tcGameObject::LoadFromFile(file);
	
    wxASSERT(false);
}

/**
*
*/
void tcTorpedoObject::Serialize(tcFile& file, bool mbLoad) 
{
	if (mbLoad) 
	{
		LoadFromFile(file);
	}
	else 
	{
		SaveToFile(file);
	}
}

/**
*
*/
tcTorpedoObject::tcTorpedoObject() 
: tcWeaponObject(),
	guidanceUpdateInterval(1.0f),
	lastGuidanceUpdate(0.0f),
	interceptTime(9999.0f),
    runTime(0),
	currentSegment(0),
	rangeToObjective_km(9999.0f),
	goalPitch_rad(0),
    goalSpeed_kts(35),
    searchHeading_rad(0),
    searchMode(SEARCH_S),
    mpDBObject(0)
{
	Clear();

	seeker.mnMode = SSMODE_SEEKERSEARCH;
	mnModelType = MTYPE_TORPEDO;
}

/**
* Copy constructor.
*/
tcTorpedoObject::tcTorpedoObject(tcTorpedoObject& o) 
: tcWeaponObject(o)
{
	mnModelType = MTYPE_TORPEDO;
	goalHeading_rad = o.goalHeading_rad;
	goalPitch_rad = o.goalPitch_rad;
	goalSpeed_kts = o.goalSpeed_kts; 
	interceptTime = o.interceptTime;
    runTime = o.runTime;

	lastGuidanceUpdate = o.lastGuidanceUpdate;
	guidanceUpdateInterval = o.guidanceUpdateInterval;
	waypoint = o.waypoint;   
	rangeToObjective_km = o.rangeToObjective_km; 
	currentSegment = o.currentSegment;
    seeker = o.seeker;
	mpDBObject = o.mpDBObject;
}
/**
* Constructor that initializes using info from database entry.
*/
tcTorpedoObject::tcTorpedoObject(tcTorpedoDBObject* obj)
: tcWeaponObject(obj),
	guidanceUpdateInterval(1.0f),
	lastGuidanceUpdate(0.0f),
	interceptTime(9999.0f),
    runTime(0),
	currentSegment(0),
	rangeToObjective_km(9999.0f),
	goalPitch_rad(0),
    goalSpeed_kts(35),
    searchHeading_rad(0),
    searchMode(SEARCH_S),
    mpDBObject(obj)
{
	mnModelType = MTYPE_TORPEDO;


	// init seeker
    tcDatabaseObject* sonarDBObj = database->GetObject(obj->sonarClass);

	if (sonarDBObj)
	{
        seeker.InitFromDatabase(sonarDBObj->mnKey); 
		seeker.SetParent(this);
		seeker.SetMountAz(0); // torpedo seeker always points forward
	}
    else
    {
        fprintf(stderr, "Error - tcTorpedoObject::tcTorpedoObject(tcTorpedoDBObject* obj)"
            " - Bad or missing sensor for torpedo seeker\n");
    }
}

/**
*
*/
tcTorpedoObject::~tcTorpedoObject() 
{
}