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