void WBState::drawGL()
{
glDisable(GL_LIGHTING);
glPointSize(2);
glColor3f(1,1,0);
glBegin(GL_POINTS);
glVertex3f(pose.x,pose.y,pose.z);
glEnd();
glColor3f(0,0,1);
int num=getNumSectors();
for(int i=0;i<num;i++)sectors[i].drawGL();
}
double WBState::getClosestAngle(double ang)
{
double retangle, minimum=2*PI,val;
retangle=ang=Angle::makePositive(ang);
int num=getNumSectors();
for(int i=0;i<num;i++)
{
if((sectors[i].min<=ang)&&(sectors[i].max>=ang))return ang;
if((sectors[i].min<ang)&&((val=ang-sectors[i].max)<minimum)){
retangle=sectors[i].max;
minimum=val;
}
if((sectors[i].max>ang)&&((val=sectors[i].min-ang)<minimum)){
retangle=sectors[i].min;
minimum=val;
}
}
return retangle;
}
static void executeCommand(HarddiskIde* hd, UInt8 cmd)
{
hd->statusReg &= ~(STATUS_DRQ | STATUS_ERR);
hd->transferRead = 0;
hd->transferWrite = 0;
switch (cmd) {
case 0xef: // Set Feature
if (hd->featureReg != 0x03) {
setError(hd, 0x04);
}
break;
case 0xec: // ATA Identify Device
if (diskReadSector(hd->diskId, hd->sectorData, -1, 0, 0, 0, NULL) != DSKE_OK) {
setError(hd, 0x44);
break;
}
hd->transferCount = 512/2;
hd->sectorDataOffset = 0;
hd->transferRead = 1;
hd->statusReg |= STATUS_DRQ;
break;
case 0x91:
break;
case 0xf8: {
UInt32 sectorCount = diskGetSectorsPerTrack(hd->diskId);
hd->sectorNumReg = (UInt8)((sectorCount >> 0) & 0xff);
hd->cylinderLowReg = (UInt8)((sectorCount >> 8) & 0xff);
hd->cylinderHighReg = (UInt8)((sectorCount >> 16) & 0xff);
hd->devHeadReg = (UInt8)((sectorCount >> 24) & 0x0f);
break;
}
case 0x30: { // Write Sector
int sectorNumber = getSectorNumber(hd);
int numSectors = getNumSectors(hd);
if ((sectorNumber + numSectors) > diskGetSectorsPerTrack(hd->diskId)) {
setError(hd, 0x14);
break;
}
hd->transferSectorNumber = sectorNumber;
hd->transferCount = 512/2 * numSectors;
hd->sectorDataOffset = 0;
hd->transferWrite = 1;
hd->statusReg |= STATUS_DRQ;
break;
}
case 0x20: { // Read Sector
int sectorNumber = getSectorNumber(hd);
int numSectors = getNumSectors(hd);
int i;
if ((sectorNumber + numSectors) > diskGetSectorsPerTrack(hd->diskId)) {
setError(hd, 0x14);
break;
}
for (i = 0; i < numSectors; i++) {
if (diskReadSector(hd->diskId, hd->sectorData + i * 512, sectorNumber + i + 1, 0, 0, 0, NULL) != DSKE_OK) {
break;
}
}
if (i != numSectors) {
setError(hd, 0x44);
break;
}
hd->transferCount = 512/2 * numSectors;
hd->sectorDataOffset = 0;
hd->transferRead = 1;
hd->statusReg |= STATUS_DRQ;
break;
}
default: // all others
setError(hd, 0x04);
}
}
