void vkmobLog::enter(string entry_,
vkmobLogMask face_ = vkmobLogDefaultFace, string a_, string b_, string c_,
string d_) {
if (_logs) {
size_t logPos(0);
vector<vkmobLog*>::iterator it;
for (it = _logs->begin(); it != _logs->end(); it++) {
#ifdef vkDefHaveBitset
bool faceBit(face_[logPos]);
bool maskBit(_mask[logPos]);
if (faceBit && maskBit) {
#else
char faceBit(face_[logPos]);
char maskBit(_mask[logPos]);
if ((faceBit == '1') && (maskBit == '1')) {
#endif
(*it)->enter(entry_, face_, a_, b_, c_, d_);
}
logPos++;
}
}
}
vkmobLogMask vkmobLog::getMask() {
return _mask;
}
void vkmobLog::setMask(vkmobLogMask mask_) {
_mask = mask_;
}
void ExplainCanvas::OnMouseMotion(wxMouseEvent &ev)
{
ev.Skip(true);
if (ev.Dragging())
return;
wxClientDC dc(this);
PrepareDC(dc);
wxPoint logPos(ev.GetLogicalPosition(dc));
double x, y;
x = (double) logPos.x;
y = (double) logPos.y;
// Find the nearest object
int attachment = 0;
ExplainShape *nearestObj = dynamic_cast<ExplainShape *>(FindShape(x, y, &attachment));
if (nearestObj)
{
ShowPopup(nearestObj);
}
}
void wxShapeCanvas::OnMouseEvent(wxMouseEvent& event)
{
wxClientDC dc(this);
PrepareDC(dc);
wxPoint logPos(event.GetLogicalPosition(dc));
double x, y;
x = (double) logPos.x;
y = (double) logPos.y;
int keys = 0;
if (event.ShiftDown())
keys = keys | KEY_SHIFT;
if (event.ControlDown())
keys = keys | KEY_CTRL;
bool dragging = event.Dragging();
// Check if we're within the tolerance for mouse movements.
// If we're very close to the position we started dragging
// from, this may not be an intentional drag at all.
if (dragging)
{
int dx = abs(dc.LogicalToDeviceX((long) (x - m_firstDragX)));
int dy = abs(dc.LogicalToDeviceY((long) (y - m_firstDragY)));
if (m_checkTolerance && (dx <= GetDiagram()->GetMouseTolerance()) && (dy <= GetDiagram()->GetMouseTolerance()))
{
return;
}
else
// If we've ignored the tolerance once, then ALWAYS ignore
// tolerance in this drag, even if we come back within
// the tolerance range.
m_checkTolerance = false;
}
// Dragging - note that the effect of dragging is left entirely up
// to the object, so no movement is done unless explicitly done by
// object.
if (dragging && m_draggedShape && m_dragState == StartDraggingLeft)
{
m_dragState = ContinueDraggingLeft;
// If the object isn't m_draggable, transfer message to canvas
if (m_draggedShape->Draggable())
m_draggedShape->GetEventHandler()->OnBeginDragLeft((double)x, (double)y, keys, m_draggedAttachment);
else
{
m_draggedShape = NULL;
OnBeginDragLeft((double)x, (double)y, keys);
}
m_oldDragX = x; m_oldDragY = y;
}
else if (dragging && m_draggedShape && m_dragState == ContinueDraggingLeft)
{
// Continue dragging
m_draggedShape->GetEventHandler()->OnDragLeft(false, m_oldDragX, m_oldDragY, keys, m_draggedAttachment);
m_draggedShape->GetEventHandler()->OnDragLeft(true, (double)x, (double)y, keys, m_draggedAttachment);
m_oldDragX = x; m_oldDragY = y;
}
else if (event.LeftUp() && m_draggedShape && m_dragState == ContinueDraggingLeft)
{
m_dragState = NoDragging;
m_checkTolerance = true;
m_draggedShape->GetEventHandler()->OnDragLeft(false, m_oldDragX, m_oldDragY, keys, m_draggedAttachment);
m_draggedShape->GetEventHandler()->OnEndDragLeft((double)x, (double)y, keys, m_draggedAttachment);
m_draggedShape = NULL;
}
else if (dragging && m_draggedShape && m_dragState == StartDraggingRight)
{
m_dragState = ContinueDraggingRight;
if (m_draggedShape->Draggable())
m_draggedShape->GetEventHandler()->OnBeginDragRight((double)x, (double)y, keys, m_draggedAttachment);
else
{
m_draggedShape = NULL;
OnBeginDragRight((double)x, (double)y, keys);
}
m_oldDragX = x; m_oldDragY = y;
}
else if (dragging && m_draggedShape && m_dragState == ContinueDraggingRight)
{
// Continue dragging
m_draggedShape->GetEventHandler()->OnDragRight(false, m_oldDragX, m_oldDragY, keys, m_draggedAttachment);
m_draggedShape->GetEventHandler()->OnDragRight(true, (double)x, (double)y, keys, m_draggedAttachment);
m_oldDragX = x; m_oldDragY = y;
}
else if (event.RightUp() && m_draggedShape && m_dragState == ContinueDraggingRight)
{
m_dragState = NoDragging;
m_checkTolerance = true;
m_draggedShape->GetEventHandler()->OnDragRight(false, m_oldDragX, m_oldDragY, keys, m_draggedAttachment);
m_draggedShape->GetEventHandler()->OnEndDragRight((double)x, (double)y, keys, m_draggedAttachment);
m_draggedShape = NULL;
}
// All following events sent to canvas, not object
else if (dragging && !m_draggedShape && m_dragState == StartDraggingLeft)
{
m_dragState = ContinueDraggingLeft;
OnBeginDragLeft((double)x, (double)y, keys);
m_oldDragX = x; m_oldDragY = y;
}
else if (dragging && !m_draggedShape && m_dragState == ContinueDraggingLeft)
{
// Continue dragging
OnDragLeft(false, m_oldDragX, m_oldDragY, keys);
OnDragLeft(true, (double)x, (double)y, keys);
m_oldDragX = x; m_oldDragY = y;
}
else if (event.LeftUp() && !m_draggedShape && m_dragState == ContinueDraggingLeft)
{
m_dragState = NoDragging;
m_checkTolerance = true;
OnDragLeft(false, m_oldDragX, m_oldDragY, keys);
OnEndDragLeft((double)x, (double)y, keys);
m_draggedShape = NULL;
}
else if (dragging && !m_draggedShape && m_dragState == StartDraggingRight)
{
m_dragState = ContinueDraggingRight;
OnBeginDragRight((double)x, (double)y, keys);
m_oldDragX = x; m_oldDragY = y;
}
else if (dragging && !m_draggedShape && m_dragState == ContinueDraggingRight)
{
// Continue dragging
OnDragRight(false, m_oldDragX, m_oldDragY, keys);
OnDragRight(true, (double)x, (double)y, keys);
m_oldDragX = x; m_oldDragY = y;
}
else if (event.RightUp() && !m_draggedShape && m_dragState == ContinueDraggingRight)
{
m_dragState = NoDragging;
m_checkTolerance = true;
OnDragRight(false, m_oldDragX, m_oldDragY, keys);
OnEndDragRight((double)x, (double)y, keys);
m_draggedShape = NULL;
}
// Non-dragging events
else if (event.IsButton())
{
m_checkTolerance = true;
// Find the nearest object
int attachment = 0;
wxShape *nearest_object = FindShape(x, y, &attachment);
if (nearest_object) // Object event
{
if (event.LeftDown())
{
m_draggedShape = nearest_object;
m_draggedAttachment = attachment;
m_dragState = StartDraggingLeft;
m_firstDragX = x;
m_firstDragY = y;
}
else if (event.LeftUp())
{
// N.B. Only register a click if the same object was
// identified for down *and* up.
if (nearest_object == m_draggedShape)
nearest_object->GetEventHandler()->OnLeftClick((double)x, (double)y, keys, attachment);
m_draggedShape = NULL;
m_dragState = NoDragging;
}
else if (event.LeftDClick())
{
nearest_object->GetEventHandler()->OnLeftDoubleClick((double)x, (double)y, keys, attachment);
m_draggedShape = NULL;
m_dragState = NoDragging;
}
else if (event.RightDown())
{
m_draggedShape = nearest_object;
m_draggedAttachment = attachment;
m_dragState = StartDraggingRight;
m_firstDragX = x;
m_firstDragY = y;
}
else if (event.RightUp())
{
if (nearest_object == m_draggedShape)
nearest_object->GetEventHandler()->OnRightClick((double)x, (double)y, keys, attachment);
m_draggedShape = NULL;
m_dragState = NoDragging;
}
}
else // Canvas event (no nearest object)
{
if (event.LeftDown())
{
m_draggedShape = NULL;
m_dragState = StartDraggingLeft;
m_firstDragX = x;
m_firstDragY = y;
}
else if (event.LeftUp())
{
OnLeftClick((double)x, (double)y, keys);
m_draggedShape = NULL;
m_dragState = NoDragging;
}
else if (event.RightDown())
{
m_draggedShape = NULL;
m_dragState = StartDraggingRight;
m_firstDragX = x;
m_firstDragY = y;
}
else if (event.RightUp())
{
OnRightClick((double)x, (double)y, keys);
m_draggedShape = NULL;
m_dragState = NoDragging;
}
}
}
}
int main(){
scatter();
cout << "Gaussian " << gauss_rand() << endl;
ALMcl core;
core.init(10,"landmarks.dat");
drovector pos_(4);
pos_.zero();
cout << "landmark 0 " << core.vision_simulator(pos_,rovec_read(core.landmarks,0)) <<endl;
cout << "landmark 1 " << core.vision_simulator(pos_,rovec_read(core.landmarks,1)) <<endl;
cout << "landmark 2 " << core.vision_simulator(pos_,rovec_read(core.landmarks,2)) <<endl;
cout << "landmark 3 " << core.vision_simulator(pos_,rovec_read(core.landmarks,3)) <<endl;
//ここまでdebug
ALMcl engine, sim, pure;//engine = localization engine, sim = real world simulator
//pure is for simulation excluding noise term
// pure.forward_noise = 0;
//pure.theta_noise = 0;
drovector pos(4),purepos(4);
pos.zero();purepos.zero();
engine.init(100,"landmarks.dat");
dgematrix senario;
senario.read("senario.txt");
dgematrix logPos(T,4),logPure(T,4),logEst(T,4);
sim.forward_noise = 1.0;
sim.theta_noise = 0.1;
sim.vision_noise = 1.0;
pure.forward_noise = 0;
pure.theta_noise = 0;
pure.vision_noise = 1.0;
engine.forward_noise = 1.0;
engine.theta_noise = 0.1;
engine.vision_noise = 1.0;
cout <<"sim fn " << sim.forward_noise << endl;
cout <<"sim tn " << sim.theta_noise << endl;
cout <<"pure fn " << pure.forward_noise << endl;
cout <<"pure tn " << pure.theta_noise << endl;
for(int t=0;t<T;t++){
//command and move
pos = sim.move(pos,senario(t,0),senario(t,1),1);
cout << "pos " << pos << endl;
//pure movement
purepos = pure.move(purepos,senario(t,0),senario(t,1),1);
cout << "purepos " << purepos << endl;
// getchar();
//obtaining view
drovector vision = engine.vision_simulator(pos,rovec_read(engine.landmarks,t%4)); //each time t%4 landmark is observed
cout << "landmark " << vision << endl;
dgematrix view(0,2);// vec_set(view,0,vision);
//updating localization engine
engine.update(senario(t,0),senario(t,1),1,view);
cout << "updated particles "<< endl << engine.X << endl;
cout << "updated weight " << endl << engine.w << endl;
getchar();//estimated self position
engine.get_pos();
//loginmg
vec_set(logPos,t,pos);
vec_set(logPure,t,purepos);
vec_set(logEst,t,engine.get_pos());
}
logPos.write("logPos.txt");
logPure.write("logPure.txt");
logEst.write("logEst.txt");
return 0;
}
void CGraphView::OnMouseEvent(wxMouseEvent &event)
{
if(m_nMode==ARROW) {
wxShapeCanvas::OnMouseEvent(event);
return;
}
wxClientDC dc(this);
PrepareDC(dc);
int ch,cw;
dc.GetSize(&cw,&ch);
wxPoint logPos(event.GetLogicalPosition(dc));
double x, y;
x = (double) logPos.x;
y = (double) logPos.y;
int keys = 0;
if (event.ShiftDown())
keys = keys | KEY_SHIFT;
if (event.ControlDown())
keys = keys | KEY_CTRL;
bool dragging = event.Dragging();
// Find the nearest object
if(event.IsButton()) {
if(event.RightDown()) {
if(m_pHoverShape) {
CNodeEvent evt;
evt.SetEventType(GV_NODE_MENU);
evt.SetNode(m_pHoverShape->GetId());
evt.SetPosition(event.GetPosition());
AddPendingEvent(evt);
}
} else if(event.LeftDClick()) {
if(m_pHoverShape) {
CNodeEvent evt;
evt.SetEventType(GV_NODE_ACTIVATED);
evt.SetNode(m_pHoverShape->GetId());
evt.SetPosition(event.GetPosition());
AddPendingEvent(evt);
}
} else if(event.LeftDown()) {
if(m_pHoverShape) {
m_pClickShape=m_pHoverShape;
return;
} else {
m_pClickShape=NULL;
if(event.ShiftDown()) {
//m_bZooming=true;
//m_ptGrab=event.GetPosition();
//UpdateCursor();
//CaptureMouse();
} else {
m_bGrabbing=true;
m_ptGrab=event.GetPosition();
int vx,vy;
GetViewStart(&vx,&vy);
m_ptGrabLogical=wxPoint(vx,vy);
UpdateCursor();
CaptureMouse();
}
return;
}
} else if(event.LeftUp()) {
if(m_pClickShape && (m_pClickShape==m_pHoverShape)) {
CNodeEvent evt;
evt.SetEventType(GV_NODE_SELECTED);
evt.SetNode(m_pHoverShape->GetId());
evt.SetPosition(event.GetPosition());
AddPendingEvent(evt);
return;
}
if(m_bGrabbing || m_bZooming) {
ReleaseMouse();
}
m_bZooming=false;
m_bGrabbing=false;
UpdateCursor();
}
// if(nearest_object) {
// }
return;
} else if(event.Moving()) {
if(m_bGrabbing) {
int nx,ny;
event.GetPosition(&nx,&ny);
nx=nx-m_ptGrab.x;
ny=ny-m_ptGrab.y;
int vx,vy;
vx=m_ptGrabLogical.x-nx;
vy=m_ptGrabLogical.y-ny;
Scroll(vx,vy);
UpdateCursor();
} else if(m_bZooming) {
/* int nx,ny;
event.GetPosition(&nx,&ny);
nx=nx-m_ptGrab.x;
ny=ny-m_ptGrab.y;
double scale;
if(ny>0.0) {
scale=1.0+3.0*(ny/(ch/2.0));
} else {
scale=1.0-0.75*(-ny/(ch/2.0));
}
int vsx,vsy;
GetViewStart(&vsx,&vsy);
vsx=vsx*scale/m_diag.GetZoomFactor();
vsy=vsy*scale/m_diag.GetZoomFactor();
m_diag.SetZoomFactor(scale);
SetScrollbars(1,1,m_nGraphWidth*scale,m_nGraphHeight*scale,vsx,vsy,true);
Refresh(true);
*/
} else {
int attachment=0;
wxShape *shape=FindShape(x,y,&attachment,NULL,NULL,CLASSINFO(wxLineShape));
if(shape!=m_pHoverShape) {
m_pHoverShape=shape;
UpdateCursor();
}
}
} else if(event.Entering()) {
UpdateCursor();
}
}