//mouse event detection
void DrawingManager::mouseDrag(double x, double y){
if (mode == DRAW_M); //addPoint(x,y);
else if (mode == ELLIPSE_M || mode== LINE_M || mode==RECT_M) sizeShape(x,y);
else if (mode == SELECT_M) checkMove(x,y);
else if (mode == DIRECT_M) checkDMove(x,y);
else if (mode == SCALE_M) checkScale(x,y);
}
QPointF XmlLoader::readXandY(QDomElement const &docItem)
{
initListScalePoint();
QPair<QString, bool> pointX = readScaleCoord("x", docItem);
QPair<QString, bool> pointY = readScaleCoord("y", docItem);
qreal x = pointX.first.toDouble() + mDrift.x();
qreal y = pointY.first.toDouble() + mDrift.y();
checkScale(pointX, QPair<QString, bool>("", false), pointY, QPair<QString, bool>("", false));
return QPointF(x, y);
}
cv::Mat & ScaleSpaceImage::getImageForScale(unsigned int scale, unsigned int image_number)
{
if (image_number >= nr_images)
{
throw ScaleSpaceImageException("Wrong image number parameter: " + std::to_string(image_number) + ". Can be 0 -" + std::to_string(nr_images - 1));
}
checkScale(scale);
if (!scale_space_images[image_number][scale].isContinuous())
{
throw ScaleSpaceImageException("Data is not continuous");
}
return scale_space_images[image_number][scale];
}
QRectF XmlLoader::readRectOfXandY(QDomElement const &docItem)
{
initListScalePoint();
QPair<QString, bool> pointX1 = readScaleCoord("x1", docItem);
QPair<QString, bool> pointX2 = readScaleCoord("x2", docItem);
QPair<QString, bool> pointY1 = readScaleCoord("y1", docItem);
QPair<QString, bool> pointY2 = readScaleCoord("y2", docItem);
qreal x1 = pointX1.first.toDouble() + mDrift.x();
qreal x2 = pointX2.first.toDouble() + mDrift.x();
qreal y1 = pointY1.first.toDouble() + mDrift.y();
qreal y2 = pointY2.first.toDouble() + mDrift.y();
checkScale(pointX1, pointX2, pointY1, pointY2);
return QRectF(x1, y1, x2 - x1, y2 - y1);
}
void CK3::_initialize()
{
if (_initialized)
return;
checkScale();
// We need to find the handle of K3's motors and sensors according to data tags attached to objects.
// Since there might be several K3s in the scene, we should only explore this K3's tree hierarchy
// to find those tags:
for (int i=0;i<2;i++)
_k3MotorHandles[i]=-1;
for (int i=0;i<2;i++)
_k3ColorSensorHandles[i]=-1;
for (int i=0;i<9;i++)
_k3IrSensorHandles[i]=-1;
for (int i=0;i<5;i++)
_k3UsSensorHandles[i]=-1;
for (int i=0;i<6;i++)
_k3GripperArmMotorHandles[i]=-1;
for (int i=0;i<3;i++)
_k3GripperFingerMotorHandles[i]=-1;
for (int i=0;i<2;i++)
_k3GripperDistanceSensorHandles[i]=-1;
for (int i=0;i<2;i++)
_k3GripperColorSensorHandles[i]=-1;
std::vector<int> toExplore;
toExplore.push_back(_associatedObjectID); // We start exploration with the base of the K3-tree
while (toExplore.size()!=0)
{
int objHandle=toExplore[toExplore.size()-1];
toExplore.pop_back();
// 1. Add this object's children to the list to explore:
int index=0;
int childHandle=simGetObjectChild(objHandle,index++);
while (childHandle!=-1)
{
toExplore.push_back(childHandle);
childHandle=simGetObjectChild(objHandle,index++);
}
// 2. Now check if this object has one of the tags we are looking for:
// a. Get all the developer data attached to this scene object (this is custom data added by the developer):
int buffSize=simGetObjectCustomDataLength(objHandle,DEVELOPER_DATA_HEADER);
if (buffSize!=0)
{ // Yes there is some custom data written by us (the developer with the DEVELOPER_DATA_HEADER header)
char* datBuff=new char[buffSize];
simGetObjectCustomData(objHandle,DEVELOPER_DATA_HEADER,datBuff);
std::vector<unsigned char> developerCustomData(datBuff,datBuff+buffSize);
delete[] datBuff;
// b. From that retrieved data, try to extract sub-data with the searched tags:
std::vector<unsigned char> k3TagData;
if (CAccess::extractSerializationData(developerCustomData,K3_LEFTMOTOR,k3TagData))
_k3MotorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_RIGHTMOTOR,k3TagData))
_k3MotorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_COLORSENSORLEFT,k3TagData))
_k3ColorSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_COLORSENSORRIGHT,k3TagData))
_k3ColorSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR1,k3TagData))
_k3IrSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR2,k3TagData))
_k3IrSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR3,k3TagData))
_k3IrSensorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR4,k3TagData))
_k3IrSensorHandles[3]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR5,k3TagData))
_k3IrSensorHandles[4]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR6,k3TagData))
_k3IrSensorHandles[5]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR7,k3TagData))
_k3IrSensorHandles[6]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR8,k3TagData))
_k3IrSensorHandles[7]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_IRSENSOR9,k3TagData))
_k3IrSensorHandles[8]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR1,k3TagData))
_k3UsSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR2,k3TagData))
_k3UsSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR3,k3TagData))
_k3UsSensorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR4,k3TagData))
_k3UsSensorHandles[3]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_USSENSOR5,k3TagData))
_k3UsSensorHandles[4]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR1,k3TagData))
_k3GripperArmMotorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR2,k3TagData))
_k3GripperArmMotorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR3,k3TagData))
_k3GripperArmMotorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR4,k3TagData))
_k3GripperArmMotorHandles[3]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR5,k3TagData))
_k3GripperArmMotorHandles[4]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_ARMMOTOR6,k3TagData))
_k3GripperArmMotorHandles[5]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR1,k3TagData))
_k3GripperFingerMotorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR2,k3TagData))
_k3GripperFingerMotorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_GRIPPERMOTOR3,k3TagData))
_k3GripperFingerMotorHandles[2]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_LEFTDISTSENSOR,k3TagData))
_k3GripperDistanceSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_RIGHTDISTSENSOR,k3TagData))
_k3GripperDistanceSensorHandles[1]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_LEFTCOLSENSOR,k3TagData))
_k3GripperColorSensorHandles[0]=objHandle;
if (CAccess::extractSerializationData(developerCustomData,K3_GRIPPER_RIGHTCOLSENSOR,k3TagData))
_k3GripperColorSensorHandles[1]=objHandle;
}
}
// Now get the display handle (Custom UI):
char* baseName=simGetObjectName(_associatedObjectID);
int suffixNumber=simGetNameSuffix(baseName);
simReleaseBuffer(baseName);
int globalSuffixSave=simGetNameSuffix(NULL);
simSetNameSuffix(suffixNumber);
_k3DisplayHandle=simGetUIHandle("K3_stateVisualization");
simSetNameSuffix(globalSuffixSave); // reset to previous value
_targetVelocities[0]=0.0f;
_targetVelocities[1]=0.0f;
_currentVelocities[0]=0.0f;
_currentVelocities[1]=0.0f;
_cumulativeMotorAngles[0]=0.0f;
_cumulativeMotorAngles[1]=0.0f;
_previousMotorAngles[0]=0.0f;
_previousMotorAngles[1]=0.0f;
_currentArmPosition=0.0f;
_targetArmPosition=0.0f;
_currentArmVelocity=0.0f;
_targetGripperGap_unscaled=0.055f;
_initialized=true;
}
cv::Mat & ScaleSpaceImage::getOutput(unsigned int scale)
{
checkScale(scale);
return output[scale];
}
void * ScaleSpaceImage::getDataForOutput(unsigned int scale)
{
checkScale(scale);
return output[scale].data;
}
