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);
}
