forked from speed-dreams/speed-dreams-code
- replace default logging by LogDANDROID on this driver
git-svn-id: https://svn.code.sf.net/p/speed-dreams/code/trunk@7085 30fe4595-0a0c-4342-8851-515496e4dcbd Former-commit-id: b033e8639ded0686d7093a4336a4498e98335873 Former-commit-id: 45578095341d79afef6f3162753401aded819e44
This commit is contained in:
torcs-ng
parent
2ce184fb36
commit
35d96145e8
4 changed files with 2059 additions and 1895 deletions
|
|
@ -19,12 +19,17 @@
|
|||
#include <windows.h>
|
||||
#endif
|
||||
|
||||
#include <tgf.h>
|
||||
#include <robot.h> // ROB_IDENT
|
||||
|
||||
#include <string>
|
||||
#include <vector>
|
||||
#include <utility>
|
||||
#include "driver.h"
|
||||
|
||||
// The "DANDROID" logger instance
|
||||
GfLogger* PLogDANDROID = 0;
|
||||
|
||||
using ::std::string;
|
||||
using ::std::vector;
|
||||
using ::std::pair;
|
||||
|
|
@ -70,22 +75,19 @@ static int indexOffset = 0;
|
|||
// in the robot's xml-file between others, not only at the end of the list
|
||||
const char *sUndefined = "undefined";
|
||||
|
||||
|
||||
|
||||
////////////////////////////////
|
||||
// Utility
|
||||
////////////////////////////////
|
||||
#ifdef DANDROID_SPEEDDREAMS
|
||||
|
||||
// Set robots's name and xml file pathname
|
||||
static void setRobotName(const string &name) {
|
||||
static void setRobotName(const string &name)
|
||||
{
|
||||
char buffer[BUFSIZE];
|
||||
snprintf(buffer, BUFSIZE, "drivers/%s/%s.xml", name.c_str(), name.c_str());
|
||||
snprintf(buffer, BUFSIZE, "drivers/%s/%s.xml", name.c_str(), name.c_str());
|
||||
nameBuffer = name;
|
||||
pathBuffer = buffer;
|
||||
}
|
||||
|
||||
|
||||
////////////////////////////////////////////////////////////////
|
||||
// SD Interface (new, fixed name scheme, from Andrew's USR code)
|
||||
////////////////////////////////////////////////////////////////
|
||||
|
|
@ -94,18 +96,22 @@ static void setRobotName(const string &name) {
|
|||
// Extended for use with schismatic robots
|
||||
|
||||
extern "C" int moduleWelcome(const tModWelcomeIn* welcomeIn,
|
||||
tModWelcomeOut* welcomeOut) {
|
||||
tModWelcomeOut* welcomeOut)
|
||||
{
|
||||
// Save module name and loadDir, and determine module XML file pathname.
|
||||
setRobotName(welcomeIn->name);
|
||||
|
||||
// Filehandle for robot's xml-file
|
||||
void *pRobotSettings = GfParmReadFile(pathBuffer.c_str(), GFPARM_RMODE_STD);
|
||||
|
||||
if (pRobotSettings) { // robot settings XML could be read
|
||||
PLogDANDROID = GfLogger::instance("DANDROID");
|
||||
|
||||
if (pRobotSettings) // robot settings XML could be read
|
||||
{
|
||||
NBBOTS = 0;
|
||||
|
||||
char SectionBuffer[BUFSIZE];
|
||||
snprintf(SectionBuffer, BUFSIZE, "%s/%s/%d", ROB_SECT_ROBOTS, ROB_LIST_INDEX, 0);
|
||||
snprintf(SectionBuffer, BUFSIZE, "%s/%s/%d", ROB_SECT_ROBOTS, ROB_LIST_INDEX, 0);
|
||||
|
||||
// Try to get first driver from index 0
|
||||
const string sDriverName = GfParmGetStrNC(pRobotSettings,
|
||||
|
|
@ -126,7 +132,7 @@ extern "C" int moduleWelcome(const tModWelcomeIn* welcomeIn,
|
|||
// save defined driver names and descriptions.
|
||||
Drivers.clear();
|
||||
for (int i = indexOffset; i < MAXNBBOTS + indexOffset; ++i) {
|
||||
snprintf(SectionBuffer, BUFSIZE, "%s/%s/%d", ROB_SECT_ROBOTS, ROB_LIST_INDEX, i);
|
||||
snprintf(SectionBuffer, BUFSIZE, "%s/%s/%d", ROB_SECT_ROBOTS, ROB_LIST_INDEX, i);
|
||||
|
||||
string sDriverName = GfParmGetStr(pRobotSettings, SectionBuffer,
|
||||
ROB_ATTR_NAME, sUndefined);
|
||||
|
|
@ -151,11 +157,10 @@ extern "C" int moduleWelcome(const tModWelcomeIn* welcomeIn,
|
|||
|
||||
return 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
// Module entry point (new fixed name scheme).
|
||||
extern "C" int moduleInitialize(tModInfo *modInfo) {
|
||||
extern "C" int moduleInitialize(tModInfo *modInfo)
|
||||
{
|
||||
// Clear all structures.
|
||||
memset(modInfo, 0, NBBOTS * sizeof(tModInfo));
|
||||
for (int i = 0; i < NBBOTS; i++) {
|
||||
|
|
@ -199,7 +204,7 @@ extern "C" int dandroid(tModInfo *modInfo) {
|
|||
if (pRobotSettings) { // Let's look what we have to provide here
|
||||
char SectionBuffer[BUFSIZE];
|
||||
for (int i = 0; i < NBBOTS; i++) {
|
||||
snprintf(SectionBuffer, BUFSIZE, "%s/%s/%d", ROB_SECT_ROBOTS, ROB_LIST_INDEX, i);
|
||||
snprintf(SectionBuffer, BUFSIZE, "%s/%s/%d", ROB_SECT_ROBOTS, ROB_LIST_INDEX, i);
|
||||
string sDriverName = GfParmGetStr(pRobotSettings, SectionBuffer, ROB_ATTR_NAME, defaultBotName[i].c_str());
|
||||
string sDriverDesc = GfParmGetStr(pRobotSettings, SectionBuffer, ROB_ATTR_DESC, defaultBotDesc[i].c_str());
|
||||
Drivers.push_back(make_pair(sDriverName, sDriverDesc));
|
||||
|
|
@ -224,7 +229,7 @@ static int InitFuncPt(int index, void *pt) {
|
|||
// Create robot instance for index.
|
||||
driver[index] = new TDriver(index);
|
||||
driver[index]->MyBotName = nameBuffer.c_str();
|
||||
|
||||
|
||||
itf->rbNewTrack = initTrack; // Give the robot the track view called.
|
||||
itf->rbNewRace = newRace; // Start a new race.
|
||||
itf->rbDrive = drive; // Drive during race.
|
||||
|
|
|
|||
|
|
@ -23,352 +23,415 @@
|
|||
|
||||
//#define DANPATH_VERBOSE
|
||||
|
||||
// The "DANDROID" logger instance.
|
||||
extern GfLogger* PLogDANDROID;
|
||||
#define LogDANDROID (*PLogDANDROID)
|
||||
|
||||
DanLine::DanLine()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void DanLine::init(PTrack t)
|
||||
{
|
||||
MAX_RADIUS = 1000.0;
|
||||
mTrack = t;
|
||||
myseg = mTrack->seg;
|
||||
MAX_RADIUS = 1000.0;
|
||||
mTrack = t;
|
||||
myseg = mTrack->seg;
|
||||
}
|
||||
|
||||
|
||||
void DanLine::addDanPoint(const DanPoint &danpoint)
|
||||
{
|
||||
mLine.push_back(danpoint);
|
||||
mLine.push_back(danpoint);
|
||||
}
|
||||
|
||||
|
||||
bool DanLine::calcParam()
|
||||
{
|
||||
int i;
|
||||
for (i = 0; i < (int)mLine.size(); i++) {
|
||||
if (fromStart(mLine[i].pos, mLine[i].fromstart)) {
|
||||
if (!toMiddle(mLine[i].pos, mLine[i].tomiddle)) {
|
||||
return false;
|
||||
}
|
||||
} else {
|
||||
return false;
|
||||
int i;
|
||||
for (i = 0; i < (int)mLine.size(); i++)
|
||||
{
|
||||
if (fromStart(mLine[i].pos, mLine[i].fromstart))
|
||||
{
|
||||
if (!toMiddle(mLine[i].pos, mLine[i].tomiddle))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
return false;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (i = 0; i < (int)mLine.size(); i++) {
|
||||
mLine[i].yaw = calcYaw(mLine[i]);
|
||||
double trackyaw;
|
||||
if (!calcTrackYaw(mLine[i], trackyaw)) {
|
||||
return false;
|
||||
}
|
||||
mLine[i].angletotrack = mLine[i].yaw - trackyaw;
|
||||
NORM_PI_PI(mLine[i].angletotrack);
|
||||
}
|
||||
for (i = 0; i < (int)mLine.size(); i++) {
|
||||
if (fabs(mLine[i].radius) < MAX_RADIUS) {
|
||||
mLine[i].type = (SIGN(mLine[i].radius) > 0) ? TR_LFT : TR_RGT;
|
||||
} else {
|
||||
mLine[i].type = TR_STR;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
for (i = 0; i < (int)mLine.size(); i++)
|
||||
{
|
||||
mLine[i].yaw = calcYaw(mLine[i]);
|
||||
double trackyaw;
|
||||
|
||||
if (!calcTrackYaw(mLine[i], trackyaw))
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
mLine[i].angletotrack = mLine[i].yaw - trackyaw;
|
||||
NORM_PI_PI(mLine[i].angletotrack);
|
||||
}
|
||||
|
||||
for (i = 0; i < (int)mLine.size(); i++)
|
||||
{
|
||||
if (fabs(mLine[i].radius) < MAX_RADIUS)
|
||||
{
|
||||
mLine[i].type = (SIGN(mLine[i].radius) > 0) ? TR_LFT : TR_RGT;
|
||||
}
|
||||
else
|
||||
{
|
||||
mLine[i].type = TR_STR;
|
||||
}
|
||||
}
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
void DanLine::createSectors(std::vector <DanSector>& sect)
|
||||
{
|
||||
int sector = 0;
|
||||
DanSector dansect;
|
||||
dansect.sector = sector;
|
||||
dansect.learned = 0;
|
||||
dansect.fromstart = 0.0;
|
||||
dansect.brakedistfactor = 1.0;
|
||||
dansect.speedfactor = 0.9;
|
||||
dansect.time = 0.0;
|
||||
dansect.bestspeedfactor = 1.0;
|
||||
dansect.besttime = 10000.0;
|
||||
sect.push_back(dansect);
|
||||
double lastfromstart = dansect.fromstart;
|
||||
bool newsector = false;
|
||||
bool largeradius = true;
|
||||
for (int i = 1 ; i < (int)mLine.size(); i++) {
|
||||
// Find sector
|
||||
if (fabs(mLine[i].radius) < 150.0) {
|
||||
largeradius = false;
|
||||
}
|
||||
if (fabs(mLine[i].radius) > 200.0) {
|
||||
if (!largeradius) {
|
||||
newsector = true;
|
||||
}
|
||||
largeradius = true;
|
||||
}
|
||||
// Store sector
|
||||
if (newsector) {
|
||||
if (mLine[mLine.size()-1].fromstart - mLine[i].fromstart > 400.0) {
|
||||
if (mLine[i].fromstart < 200.0) {
|
||||
// Do nothing
|
||||
} else if (mLine[i].fromstart - lastfromstart > 200.0) {
|
||||
sector++;
|
||||
dansect.sector = sector;
|
||||
dansect.fromstart = mLine[i].fromstart;
|
||||
lastfromstart = dansect.fromstart;
|
||||
sect.push_back(dansect);
|
||||
//GfOut("fs:%g radius:%g\n", mLine[i].fromstart, fabs(mLine[i].radius));
|
||||
} else {
|
||||
sect[sector].fromstart = mLine[i].fromstart;
|
||||
lastfromstart = mLine[i].fromstart;
|
||||
//GfOut("overwrite fs:%g radius:%g\n", mLine[i].fromstart, fabs(mLine[i].radius));
|
||||
}
|
||||
}
|
||||
newsector = false;
|
||||
}
|
||||
}
|
||||
printData();
|
||||
}
|
||||
int sector = 0;
|
||||
DanSector dansect;
|
||||
dansect.sector = sector;
|
||||
dansect.learned = 0;
|
||||
dansect.fromstart = 0.0;
|
||||
dansect.brakedistfactor = 1.0;
|
||||
dansect.speedfactor = 0.9;
|
||||
dansect.time = 0.0;
|
||||
dansect.bestspeedfactor = 1.0;
|
||||
dansect.besttime = 10000.0;
|
||||
sect.push_back(dansect);
|
||||
double lastfromstart = dansect.fromstart;
|
||||
bool newsector = false;
|
||||
bool largeradius = true;
|
||||
|
||||
for (int i = 1 ; i < (int)mLine.size(); i++)
|
||||
{
|
||||
// Find sector
|
||||
if (fabs(mLine[i].radius) < 150.0)
|
||||
{
|
||||
largeradius = false;
|
||||
}
|
||||
|
||||
if (fabs(mLine[i].radius) > 200.0)
|
||||
{
|
||||
if (!largeradius)
|
||||
{
|
||||
newsector = true;
|
||||
}
|
||||
|
||||
largeradius = true;
|
||||
}
|
||||
// Store sector
|
||||
if (newsector)
|
||||
{
|
||||
if (mLine[mLine.size()-1].fromstart - mLine[i].fromstart > 400.0)
|
||||
{
|
||||
if (mLine[i].fromstart < 200.0)
|
||||
{
|
||||
// Do nothing
|
||||
}
|
||||
else if (mLine[i].fromstart - lastfromstart > 200.0)
|
||||
{
|
||||
sector++;
|
||||
dansect.sector = sector;
|
||||
dansect.fromstart = mLine[i].fromstart;
|
||||
lastfromstart = dansect.fromstart;
|
||||
sect.push_back(dansect);
|
||||
//GfOut("fs:%g radius:%g\n", mLine[i].fromstart, fabs(mLine[i].radius));
|
||||
}
|
||||
else
|
||||
{
|
||||
sect[sector].fromstart = mLine[i].fromstart;
|
||||
lastfromstart = mLine[i].fromstart;
|
||||
//GfOut("overwrite fs:%g radius:%g\n", mLine[i].fromstart, fabs(mLine[i].radius));
|
||||
}
|
||||
}
|
||||
|
||||
newsector = false;
|
||||
}
|
||||
}
|
||||
|
||||
printData();
|
||||
}
|
||||
|
||||
void DanLine::printData()
|
||||
{
|
||||
#ifdef DANPATH_VERBOSE
|
||||
if (mLine[0].line == 0) {
|
||||
for (int i = 0; i < (int)mLine.size(); i++) {
|
||||
GfOut("ind:%d fs:%g r:%g curv_z:%g\n", i, mLine[i].fromstart, mLine[i].radius, mLine[i].curv_z);
|
||||
if (mLine[0].line == 0)
|
||||
{
|
||||
for (int i = 0; i < (int)mLine.size(); i++)
|
||||
{
|
||||
LogDANDROID.debug("ind:%d fs:%g r:%g curv_z:%g\n", i, mLine[i].fromstart, mLine[i].radius, mLine[i].curv_z);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
|
||||
bool DanLine::getDanPos(double fromstart, DanPoint& danpoint)
|
||||
{
|
||||
if (!mLine.size()) {
|
||||
return false;
|
||||
}
|
||||
int index = getIndex(fromstart);
|
||||
danpoint = mLine[index];
|
||||
if (!mLine.size())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
|
||||
// Calculate radius
|
||||
double radius = mLine[index].radius;
|
||||
double nextradius = nextPos(mLine[index]).radius;
|
||||
if (SIGN(radius) != SIGN(nextradius)) {
|
||||
danpoint.radius = 100000.0;
|
||||
} else {
|
||||
// Interpolate radius linear
|
||||
double seglength = getDistDiff(mLine[index].fromstart, nextPos(mLine[index]).fromstart);
|
||||
double poslength = getDistDiff(mLine[index].fromstart, fromstart);
|
||||
double invradius = 1.0 / radius + (poslength / seglength) * (1.0 / nextradius - 1.0 / radius);
|
||||
danpoint.radius = 1.0 / invradius;
|
||||
}
|
||||
int index = getIndex(fromstart);
|
||||
danpoint = mLine[index];
|
||||
|
||||
danpoint.tomiddle = getToMiddle(fromstart); // Interpolate cubic toMiddle
|
||||
danpoint.pos = getNearestPoint(danpoint.index, fromstart); // position (straight interpolation)
|
||||
danpoint.fromstart = fromstart;
|
||||
//danpoint.yaw = calcYaw(danpoint); // useless without interpolation
|
||||
return true;
|
||||
// Calculate radius
|
||||
double radius = mLine[index].radius;
|
||||
double nextradius = nextPos(mLine[index]).radius;
|
||||
|
||||
if (SIGN(radius) != SIGN(nextradius))
|
||||
{
|
||||
danpoint.radius = 100000.0;
|
||||
}
|
||||
else
|
||||
{
|
||||
// Interpolate radius linear
|
||||
double seglength = getDistDiff(mLine[index].fromstart, nextPos(mLine[index]).fromstart);
|
||||
double poslength = getDistDiff(mLine[index].fromstart, fromstart);
|
||||
double invradius = 1.0 / radius + (poslength / seglength) * (1.0 / nextradius - 1.0 / radius);
|
||||
danpoint.radius = 1.0 / invradius;
|
||||
}
|
||||
|
||||
danpoint.tomiddle = getToMiddle(fromstart); // Interpolate cubic toMiddle
|
||||
danpoint.pos = getNearestPoint(danpoint.index, fromstart); // position (straight interpolation)
|
||||
danpoint.fromstart = fromstart;
|
||||
//danpoint.yaw = calcYaw(danpoint); // useless without interpolation
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
DanPoint DanLine::nextPos(DanPoint danpoint)
|
||||
{
|
||||
danpoint.index++;
|
||||
return getPos(danpoint.index);
|
||||
}
|
||||
danpoint.index++;
|
||||
|
||||
return getPos(danpoint.index);
|
||||
}
|
||||
|
||||
DanPoint DanLine::prevPos(DanPoint danpoint)
|
||||
{
|
||||
danpoint.index--;
|
||||
return getPos(danpoint.index);
|
||||
}
|
||||
danpoint.index--;
|
||||
|
||||
return getPos(danpoint.index);
|
||||
}
|
||||
|
||||
DanPoint DanLine::getPos(int index)
|
||||
{
|
||||
if (index < 0) {
|
||||
return mLine[mLine.size() - 1];
|
||||
} else if (index >= (int)mLine.size()) {
|
||||
return mLine[0];
|
||||
} else {
|
||||
return mLine[index];
|
||||
}
|
||||
if (index < 0)
|
||||
{
|
||||
return mLine[mLine.size() - 1];
|
||||
}
|
||||
else if (index >= (int)mLine.size())
|
||||
{
|
||||
return mLine[0];
|
||||
}
|
||||
else
|
||||
{
|
||||
return mLine[index];
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
double DanLine::calcYaw(DanPoint danpoint)
|
||||
{
|
||||
Vec2d prev = danpoint.pos - prevPos(danpoint).pos;
|
||||
Vec2d next = nextPos(danpoint).pos - danpoint.pos;
|
||||
return Utils::VecAngle(prev + next);
|
||||
}
|
||||
Vec2d prev = danpoint.pos - prevPos(danpoint).pos;
|
||||
Vec2d next = nextPos(danpoint).pos - danpoint.pos;
|
||||
|
||||
return Utils::VecAngle(prev + next);
|
||||
}
|
||||
|
||||
double DanLine::calcTrackYaw(DanPoint danpoint, double& trackyaw)
|
||||
{
|
||||
tTrkLocPos locpos;
|
||||
RtTrackGlobal2Local(myseg, (tdble) danpoint.pos.x, (tdble) danpoint.pos.y, &locpos, TR_LPOS_MAIN);
|
||||
myseg = locpos.seg;
|
||||
trackyaw = RtTrackSideTgAngleL(&locpos);
|
||||
return true;
|
||||
}
|
||||
tTrkLocPos locpos;
|
||||
RtTrackGlobal2Local(myseg, (tdble) danpoint.pos.x, (tdble) danpoint.pos.y, &locpos, TR_LPOS_MAIN);
|
||||
myseg = locpos.seg;
|
||||
trackyaw = RtTrackSideTgAngleL(&locpos);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DanLine::fromStart(Vec2d pos, double& fromstart)
|
||||
{
|
||||
tTrkLocPos locpos;
|
||||
RtTrackGlobal2Local(myseg, (tdble) pos.x, (tdble) pos.y, &locpos, TR_LPOS_MAIN);
|
||||
myseg = locpos.seg;
|
||||
fromstart = RtGetDistFromStart2(&locpos);
|
||||
return true;
|
||||
}
|
||||
tTrkLocPos locpos;
|
||||
RtTrackGlobal2Local(myseg, (tdble) pos.x, (tdble) pos.y, &locpos, TR_LPOS_MAIN);
|
||||
myseg = locpos.seg;
|
||||
fromstart = RtGetDistFromStart2(&locpos);
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool DanLine::toMiddle(Vec2d pos, double& tomiddle)
|
||||
{
|
||||
tTrkLocPos locpos;
|
||||
RtTrackGlobal2Local(myseg, (tdble) pos.x, (tdble) pos.y, &locpos, TR_LPOS_MAIN);
|
||||
myseg = locpos.seg;
|
||||
tomiddle = locpos.toMiddle;
|
||||
return true;
|
||||
}
|
||||
tTrkLocPos locpos;
|
||||
RtTrackGlobal2Local(myseg, (tdble) pos.x, (tdble) pos.y, &locpos, TR_LPOS_MAIN);
|
||||
myseg = locpos.seg;
|
||||
tomiddle = locpos.toMiddle;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
// Find nearest section on mLine
|
||||
int DanLine::getIndex(double fromstart)
|
||||
{
|
||||
if (fromstart >= 0.0 && fromstart <= mTrack->length) {
|
||||
double estpos = fromstart / mTrack->length;
|
||||
int i = (int)floor(estpos * mLine.size());
|
||||
while (true) {
|
||||
if (i < 0) {
|
||||
i = mLine.size() - 1;
|
||||
} else if (i >= (int)mLine.size()) {
|
||||
i = 0;
|
||||
}
|
||||
double sectlen = getDistDiff(getPos(i).fromstart, getPos(i + 1).fromstart);
|
||||
// double poslen = getDistDiff(getPos(i).fromstart, fromstart);
|
||||
double poslen = getDistDiff(getPos(i).fromstart, fromstart+0.001);
|
||||
if (poslen >= 0.0 && poslen <= sectlen) {
|
||||
//GfOut("poslen:%g sectlen:%g", poslen, sectlen);
|
||||
break;
|
||||
}
|
||||
i += (int)SIGN(poslen);
|
||||
}
|
||||
return i;
|
||||
} else {
|
||||
GfOut("!!!!!!!!!!!!!There is a bug in DanLine::getIndex, 'fromstart'=%g is out of range !!!!!!!!!!!!!!!", fromstart);
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
if (fromstart >= 0.0 && fromstart <= mTrack->length)
|
||||
{
|
||||
double estpos = fromstart / mTrack->length;
|
||||
int i = (int)floor(estpos * mLine.size());
|
||||
|
||||
while (true)
|
||||
{
|
||||
if (i < 0)
|
||||
{
|
||||
i = mLine.size() - 1;
|
||||
}
|
||||
else if (i >= (int)mLine.size())
|
||||
{
|
||||
i = 0;
|
||||
}
|
||||
|
||||
double sectlen = getDistDiff(getPos(i).fromstart, getPos(i + 1).fromstart);
|
||||
// double poslen = getDistDiff(getPos(i).fromstart, fromstart);
|
||||
double poslen = getDistDiff(getPos(i).fromstart, fromstart+0.001);
|
||||
|
||||
if (poslen >= 0.0 && poslen <= sectlen)
|
||||
{
|
||||
//GfOut("poslen:%g sectlen:%g", poslen, sectlen);
|
||||
break;
|
||||
}
|
||||
|
||||
i += (int)SIGN(poslen);
|
||||
}
|
||||
|
||||
return i;
|
||||
}
|
||||
else
|
||||
{
|
||||
LogDANDROID.info("!!!!!!!!!!!!!There is a bug in DanLine::getIndex, 'fromstart'=%g is out of range !!!!!!!!!!!!!!!\n", fromstart);
|
||||
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
Vec2d DanLine::getNearestPoint(int index, double fromstart)
|
||||
{
|
||||
Vec2d straight = getPos(index+1).pos - mLine[index].pos;
|
||||
double straightlen = getDistDiff(mLine[index].fromstart, getPos(index+1).fromstart);
|
||||
double poslen = getDistDiff(mLine[index].fromstart, fromstart);
|
||||
Vec2d pointonStraight = mLine[index].pos + straight * (poslen / straightlen);
|
||||
return pointonStraight;
|
||||
}
|
||||
Vec2d straight = getPos(index+1).pos - mLine[index].pos;
|
||||
double straightlen = getDistDiff(mLine[index].fromstart, getPos(index+1).fromstart);
|
||||
double poslen = getDistDiff(mLine[index].fromstart, fromstart);
|
||||
Vec2d pointonStraight = mLine[index].pos + straight * (poslen / straightlen);
|
||||
|
||||
return pointonStraight;
|
||||
}
|
||||
|
||||
double DanLine::getToMiddle(double fromstart)
|
||||
{
|
||||
int index = getIndex(fromstart);
|
||||
TCubic ccurve(mLine[index].fromstart, mLine[index].tomiddle, mLine[index].angletotrack, nextPos(mLine[index]).fromstart, nextPos(mLine[index]).tomiddle, nextPos(mLine[index]).angletotrack);
|
||||
return ccurve.CalcOffset(fromstart);
|
||||
}
|
||||
int index = getIndex(fromstart);
|
||||
TCubic ccurve(mLine[index].fromstart, mLine[index].tomiddle, mLine[index].angletotrack, nextPos(mLine[index]).fromstart, nextPos(mLine[index]).tomiddle, nextPos(mLine[index]).angletotrack);
|
||||
|
||||
return ccurve.CalcOffset(fromstart);
|
||||
}
|
||||
|
||||
double DanLine::getDistDiff(double fromstart1, double fromstart2)
|
||||
{
|
||||
double diff = fromstart2 - fromstart1;
|
||||
diff = (diff >= 0.0) ? diff : diff + mTrack->length;
|
||||
return (diff <= mTrack->length / 2.0) ? diff : diff - mTrack->length;
|
||||
double diff = fromstart2 - fromstart1;
|
||||
diff = (diff >= 0.0) ? diff : diff + mTrack->length;
|
||||
|
||||
return (diff <= mTrack->length / 2.0) ? diff : diff - mTrack->length;
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
DanPath::DanPath()
|
||||
{
|
||||
}
|
||||
|
||||
|
||||
void DanPath::init(PTrack t, double max_left, double max_right, double marginIn, double marginOut, double factor, double seglen)
|
||||
{
|
||||
mTrack = t;
|
||||
mMaxL = max_left;
|
||||
mMaxR = max_right;
|
||||
mMarginIns = marginIn;
|
||||
mMarginOuts = marginOut;
|
||||
mClothFactor = factor;
|
||||
mSegLen = seglen;
|
||||
mTrack = t;
|
||||
mMaxL = max_left;
|
||||
mMaxR = max_right;
|
||||
mMarginIns = marginIn;
|
||||
mMarginOuts = marginOut;
|
||||
mClothFactor = factor;
|
||||
mSegLen = seglen;
|
||||
|
||||
for (int i=0; i < NUM_LINES; i++) {
|
||||
mDanLine[i].init(t);
|
||||
}
|
||||
|
||||
getClothPath();
|
||||
|
||||
for (int i=0; i < NUM_LINES; i++) {
|
||||
if (!mDanLine[i].calcParam()) {
|
||||
GfOut("Error danpath: calcParam() failed\n");
|
||||
for (int i=0; i < NUM_LINES; i++)
|
||||
{
|
||||
mDanLine[i].init(t);
|
||||
}
|
||||
}
|
||||
|
||||
mDanLine[IDEAL_LINE].createSectors(mSector);
|
||||
for (int i = 0; i < (int)mSector.size(); i++) {
|
||||
GfOut("sector:%d fs:%g speedfactor:%g\n", mSector[i].sector, mSector[i].fromstart, mSector[i].speedfactor);
|
||||
}
|
||||
getClothPath();
|
||||
|
||||
for (int i=0; i < NUM_LINES; i++)
|
||||
{
|
||||
if (!mDanLine[i].calcParam())
|
||||
{
|
||||
LogDANDROID.info("Error danpath: calcParam() failed\n");
|
||||
}
|
||||
}
|
||||
|
||||
mDanLine[IDEAL_LINE].createSectors(mSector);
|
||||
|
||||
for (int i = 0; i < (int)mSector.size(); i++)
|
||||
{
|
||||
LogDANDROID.info("sector:%d fs:%g speedfactor:%g\n", mSector[i].sector, mSector[i].fromstart, mSector[i].speedfactor);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
bool DanPath::getDanPos(int line, double fromstart, DanPoint& danpoint)
|
||||
{
|
||||
return mDanLine[line].getDanPos(fromstart, danpoint);
|
||||
return mDanLine[line].getDanPos(fromstart, danpoint);
|
||||
}
|
||||
|
||||
|
||||
DanPoint DanPath::nextPos(DanPoint danpoint)
|
||||
{
|
||||
return mDanLine[danpoint.line].nextPos(danpoint);
|
||||
return mDanLine[danpoint.line].nextPos(danpoint);
|
||||
}
|
||||
|
||||
|
||||
void DanPath::getClothPath()
|
||||
{
|
||||
Vec2d point(0, 0);
|
||||
Vec2d point(0, 0);
|
||||
|
||||
MyTrack track;
|
||||
track.NewTrack(mTrack, mSegLen);
|
||||
for (int l = 0; l < NUM_LINES; l++) {
|
||||
ClothoidPath clpath;
|
||||
if (l == IDEAL_LINE ) {
|
||||
clpath.MakeSmoothPath(&track, ClothoidPath::Options(mMaxL, mMaxR, mMarginIns, mMarginOuts, mClothFactor));
|
||||
MyTrack track;
|
||||
track.NewTrack(mTrack, mSegLen);
|
||||
|
||||
for (int l = 0; l < NUM_LINES; l++)
|
||||
{
|
||||
ClothoidPath clpath;
|
||||
if (l == IDEAL_LINE )
|
||||
{
|
||||
clpath.MakeSmoothPath(&track, ClothoidPath::Options(mMaxL, mMaxR, mMarginIns, mMarginOuts, mClothFactor));
|
||||
}
|
||||
|
||||
if (l == LEFT_LINE )
|
||||
{
|
||||
clpath.MakeSmoothPath(&track, ClothoidPath::Options(mMaxL, -0.5, 1.0, 1.5, mClothFactor));
|
||||
}
|
||||
|
||||
if (l == RIGHT_LINE)
|
||||
{
|
||||
clpath.MakeSmoothPath(&track, ClothoidPath::Options(-0.5, mMaxR, 1.0, 1.5, mClothFactor));
|
||||
}
|
||||
|
||||
LinePath::PathPt pathpoint;
|
||||
|
||||
for (int j = 0; j < track.GetSize(); j++)
|
||||
{
|
||||
pathpoint = clpath.GetAt(j);
|
||||
point.x = pathpoint.pt.x;
|
||||
point.y = pathpoint.pt.y;
|
||||
DanPoint p;
|
||||
p.line = l;
|
||||
p.index = j;
|
||||
p.pos = point;
|
||||
p.type = 0;
|
||||
p.fromstart = 0;
|
||||
p.tomiddle = 0;
|
||||
p.radius = 1/pathpoint.k;
|
||||
p.yaw = 0;
|
||||
p.angletotrack = 0;
|
||||
p.curv_z = pathpoint.kz;
|
||||
mDanLine[l].addDanPoint(p);
|
||||
}
|
||||
}
|
||||
if (l == LEFT_LINE ) {
|
||||
clpath.MakeSmoothPath(&track, ClothoidPath::Options(mMaxL, -0.5, 1.0, 1.5, mClothFactor));
|
||||
}
|
||||
if (l == RIGHT_LINE) {
|
||||
clpath.MakeSmoothPath(&track, ClothoidPath::Options(-0.5, mMaxR, 1.0, 1.5, mClothFactor));
|
||||
}
|
||||
LinePath::PathPt pathpoint;
|
||||
for (int j = 0; j < track.GetSize(); j++) {
|
||||
pathpoint = clpath.GetAt(j);
|
||||
point.x = pathpoint.pt.x;
|
||||
point.y = pathpoint.pt.y;
|
||||
DanPoint p;
|
||||
p.line = l;
|
||||
p.index = j;
|
||||
p.pos = point;
|
||||
p.type = 0;
|
||||
p.fromstart = 0;
|
||||
p.tomiddle = 0;
|
||||
p.radius = 1/pathpoint.k;
|
||||
p.yaw = 0;
|
||||
p.angletotrack = 0;
|
||||
p.curv_z = pathpoint.kz;
|
||||
mDanLine[l].addDanPoint(p);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
|
|||
File diff suppressed because it is too large
Load diff
|
|
@ -27,8 +27,12 @@
|
|||
#include "danpath.h"
|
||||
#include "pidcontroller.h"
|
||||
|
||||
// The "DANDROID" logger instance.
|
||||
extern GfLogger* PLogDANDROID;
|
||||
#define LogDANDROID (*PLogDANDROID)
|
||||
|
||||
class PathInfo {
|
||||
class PathInfo
|
||||
{
|
||||
public:
|
||||
DanPoint carpos;
|
||||
DanPoint tarpos;
|
||||
|
|
@ -37,12 +41,13 @@ class PathInfo {
|
|||
};
|
||||
|
||||
|
||||
class TDriver {
|
||||
class TDriver
|
||||
{
|
||||
public:
|
||||
TDriver(int index);
|
||||
~TDriver();
|
||||
|
||||
const char* MyBotName; // Name of this bot
|
||||
const char* MyBotName; // Name of this bot
|
||||
|
||||
void InitTrack(PTrack Track, PCarHandle CarHandle, PCarSettings *CarParmHandle, PSituation Situation);
|
||||
void NewRace(PtCarElt Car, PSituation Situation);
|
||||
|
|
@ -72,7 +77,7 @@ class TDriver {
|
|||
double getBrake(double maxspeed);
|
||||
double getAccel(double maxspeed);
|
||||
double getSteer();
|
||||
int getGear();
|
||||
int getGear();
|
||||
double getClutch();
|
||||
bool stateStuck();
|
||||
bool stateOfftrack();
|
||||
|
|
@ -150,13 +155,13 @@ class TDriver {
|
|||
tSituation* oSituation;
|
||||
tCarElt* oCar; // pointer to tCarElt struct
|
||||
double oCurrSimTime;
|
||||
|
||||
|
||||
PTrack mTrack;
|
||||
int mCarIndex;
|
||||
std::string mCarType;
|
||||
|
||||
|
||||
DanPath mDanPath;
|
||||
|
||||
|
||||
Opponents mOpponents; // the container for opponents
|
||||
Opponent* mOpp; // relevant opponent for calculations
|
||||
Opponent* mOppNear;
|
||||
|
|
|
|||
Loading…
Reference in a new issue