int buzzvm_swarm_id(struct buzzvm_s* vm) {
/* Is the swarm stack empty? */
if(buzzdarray_isempty(vm->swarmstack)) {
/* Yes, no swarm to push, push nil instead */
buzzvm_pushnil(vm);
}
else {
/* Take the stack top by default */
uint16_t stackpos = 1;
/* Do we have an argument? */
if(buzzvm_lnum(vm) > 1) {
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
stackpos = buzzvm_stack_at(vm, 1)->i.value;
}
/* Limit stackpos value to avoid out-of-bounds operations */
if(stackpos > buzzdarray_size(vm->swarmstack))
stackpos = buzzdarray_size(vm->swarmstack);
/* Push the swarm id located at stackpos */
buzzvm_pushi(vm,
buzzdarray_get(
vm->swarmstack,
buzzdarray_size(vm->swarmstack) - stackpos,
uint16_t));
}
/* Return the value */
return buzzvm_ret1(vm);
}
int BuzzDebugRayAdd(buzzvm_t vm) {
/*
* Possible signatures
* debug.rays.add(r,g,b, x,y,z)
* draws a ray from the reference point of the robot to (x,y,z).
* (x,y,z) is expressed wrt the robot reference frame
* debug.rays.add(r,g,b, x0,y0,z0, x1,y1,z1)
* draws a ray from (x0,y0,z0) to (x1,y1,z1)
* (x0,y0,z0) and (x1,y1,z1) are expressed wrt the robot reference frame
*/
CColor cColor;
CVector3 cStart, cEnd;
/* Parse arguments */
int64_t argn = buzzvm_lnum(vm);
if(argn == 6) {
/* Parse color */
buzzvm_lload(vm, 1); /* red */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 2); /* green */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 3); /* blue */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
cColor.Set(buzzvm_stack_at(vm, 3)->i.value,
buzzvm_stack_at(vm, 2)->i.value,
buzzvm_stack_at(vm, 1)->i.value);
/* Parse end vector */
buzzvm_lload(vm, 4); /* x */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 5); /* y */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 6); /* z */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
cEnd.Set(buzzvm_stack_at(vm, 3)->f.value,
buzzvm_stack_at(vm, 2)->f.value,
buzzvm_stack_at(vm, 1)->f.value);
}
else if(argn == 9) {
/* Parse color */
buzzvm_lload(vm, 1); /* red */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 2); /* green */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 3); /* blue */
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
cColor.Set(buzzvm_stack_at(vm, 3)->i.value,
buzzvm_stack_at(vm, 2)->i.value,
buzzvm_stack_at(vm, 1)->i.value);
/* Parse start vector */
buzzvm_lload(vm, 4); /* x */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 5); /* y */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 6); /* z */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
cStart.Set(buzzvm_stack_at(vm, 3)->f.value,
buzzvm_stack_at(vm, 2)->f.value,
buzzvm_stack_at(vm, 1)->f.value);
/* Parse end vector */
buzzvm_lload(vm, 7); /* x */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 8); /* y */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
buzzvm_lload(vm, 9); /* z */
buzzvm_type_assert(vm, 1, BUZZTYPE_FLOAT);
cEnd.Set(buzzvm_stack_at(vm, 3)->f.value,
buzzvm_stack_at(vm, 2)->f.value,
buzzvm_stack_at(vm, 1)->f.value);
}
else {
/* Bomb out */
buzzvm_seterror(vm, BUZZVM_ERROR_LNUM, "expected 6 or 9 arguments, but %" PRId64 " were passed", buzzvm_lnum(vm));
}
/* Get pointer to controller user data */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_USERDATA);
CBuzzController& cContr = *reinterpret_cast<CBuzzController*>(buzzvm_stack_at(vm, 1)->u.value);
/* Call method */
cContr.GetARGoSDebugInfo().RayAdd(cColor, cStart, cEnd);
return buzzvm_ret0(vm);
}
int BuzzDebugTrajectoryEnable(buzzvm_t vm) {
/*
* Possible signatures
* debug.trajectory.enable(maxpoints,r,g,b)
* enable trajectory tracking setting how many points should be stored and the drawing color
* debug.trajectory.enable(maxpoints)
* enable trajectory tracking setting how many points should be stored
* debug.trajectory.enable(r,g,b)
* enable trajectory tracking keeping maxpoints' last value and setting the drawing color
* debug.trajectory.enable()
* enable trajectory tracking keeping maxpoints' last value (default is 30)
*/
/* Get pointer to controller user data */
buzzvm_pushs(vm, buzzvm_string_register(vm, "controller", 1));
buzzvm_gload(vm);
buzzvm_type_assert(vm, 1, BUZZTYPE_USERDATA);
CBuzzController* pcContr = reinterpret_cast<CBuzzController*>(buzzvm_stack_at(vm, 1)->u.value);
/* Get last known value for max points */
SInt32 nMaxPoints = pcContr->GetARGoSDebugInfo().Trajectory.MaxPoints;
/* Parse arguments */
if(buzzvm_lnum(vm) == 4) {
/* Max points */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
nMaxPoints = buzzvm_stack_at(vm, 1)->i.value;
/* RGB drawing color */
buzzvm_lload(vm, 2);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 3);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 4);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
pcContr->GetARGoSDebugInfo().Trajectory.Color.Set(
buzzvm_stack_at(vm, 3)->i.value,
buzzvm_stack_at(vm, 2)->i.value,
buzzvm_stack_at(vm, 1)->i.value);
}
else if(buzzvm_lnum(vm) == 3) {
/* RGB drawing color */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 2);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
buzzvm_lload(vm, 3);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
pcContr->GetARGoSDebugInfo().Trajectory.Color.Set(
buzzvm_stack_at(vm, 3)->i.value,
buzzvm_stack_at(vm, 2)->i.value,
buzzvm_stack_at(vm, 1)->i.value);
}
else if(buzzvm_lnum(vm) == 1) {
/* Max points */
buzzvm_lload(vm, 1);
buzzvm_type_assert(vm, 1, BUZZTYPE_INT);
nMaxPoints = buzzvm_stack_at(vm, 1)->i.value;
}
else if(buzzvm_lnum(vm) != 0) {
/* Bomb out */
buzzvm_seterror(vm, BUZZVM_ERROR_LNUM, "expected 4, 3, or 1 arguments, but %" PRId64 " were passed", buzzvm_lnum(vm));
}
/* Call method */
CBuzzController::DebugTrajectoryEnable(pcContr, nMaxPoints);
return buzzvm_ret0(vm);
}
