int ArmKdlInverseKinematics::CartToJnt(const KDL::JntArray& q_init,
const KDL::Frame& p_in,
KDL::JntArray& q_out)
{
KDL::JntArray q_min(_min_angles.size());
KDL::JntArray q_max(_max_angles.size());
// copy the joint limits to a KDL array
for (unsigned int i = 0; i < _min_angles.size(); i++) {
q_min(i) = _min_angles[i];
q_max(i) = _max_angles[i];
}
// Forward position solver
KDL::ChainFkSolverPos_recursive fksolver1(_chain);
// Inverse velocity solver
KDL::ChainIkSolverVel_pinv iksolver1v(_chain);
// Maximum 100 iterations, stop at accuracy 1e-6
KDL::ChainIkSolverPos_NR_JL iksolverpos(_chain, q_min, q_max, fksolver1, iksolver1v, 1000, 1e-6);
int ret = iksolverpos.CartToJnt(q_init, p_in, q_out);
ROS_DEBUG("q_init: %f %f %f %f %f", q_init(0), q_init(1), q_init(2), q_init(3), q_init(4));
ROS_DEBUG("q_out: %f %f %f %f %f", q_out(0), q_out(1), q_out(2), q_out(3), q_out(4));
if (ret < 0) {
ROS_DEBUG("Inverse Kinematic found no solution. KDL Return value = %i", ret);
return -1;
} else {
ROS_DEBUG("Inverse Kinematic found a solution");
return 1;
}
}
/*
================
IN_ApplyMoveEasing
clamps coordinates to a square with coordinates +/- sqrt(2)/2, then scales them to +/- 1.
This wastes a bit of stick range, but gives the diagonals coordinates of (+/-1,+/-1),
so holding the stick on a diagonal gives the same speed boost as holding the forward and strafe keyboard keys.
================
*/
static joyaxis_t IN_ApplyMoveEasing(joyaxis_t axis)
{
joyaxis_t result = {0};
const float v = sqrtf(2.0f) / 2.0f;
result.x = q_max(-v, q_min(v, axis.x));
result.y = q_max(-v, q_min(v, axis.y));
result.x /= v;
result.y /= v;
return result;
}
/*
=================
SCR_CalcRefdef
Must be called whenever vid changes
Internal use only
=================
*/
static void SCR_CalcRefdef (void)
{
float size, scale; //johnfitz -- scale
// force the status bar to redraw
Sbar_Changed ();
scr_tileclear_updates = 0; //johnfitz
// bound viewsize
if (scr_viewsize.value < 30)
Cvar_SetQuick (&scr_viewsize, "30");
if (scr_viewsize.value > 120)
Cvar_SetQuick (&scr_viewsize, "120");
// bound fov
if (scr_fov.value < 10)
Cvar_SetQuick (&scr_fov, "10");
if (scr_fov.value > 170)
Cvar_SetQuick (&scr_fov, "170");
vid.recalc_refdef = 0;
//johnfitz -- rewrote this section
size = scr_viewsize.value;
scale = CLAMP (1.0, scr_sbarscale.value, (float)glwidth / 320.0);
if (size >= 120 || cl.intermission || scr_sbaralpha.value < 1) //johnfitz -- scr_sbaralpha.value
sb_lines = 0;
else if (size >= 110)
sb_lines = 24 * scale;
else
sb_lines = 48 * scale;
size = q_min(scr_viewsize.value, 100) / 100;
//johnfitz
//johnfitz -- rewrote this section
r_refdef.vrect.width = q_max(glwidth * size, 96); //no smaller than 96, for icons
r_refdef.vrect.height = q_min(glheight * size, glheight - sb_lines); //make room for sbar
r_refdef.vrect.x = (glwidth - r_refdef.vrect.width)/2;
r_refdef.vrect.y = (glheight - sb_lines - r_refdef.vrect.height)/2;
//johnfitz
r_refdef.fov_x = AdaptFovx(scr_fov.value, vid.width, vid.height);
r_refdef.fov_y = CalcFovy (r_refdef.fov_x, r_refdef.vrect.width, r_refdef.vrect.height);
scr_vrect = r_refdef.vrect;
}
static void S_Update_ (void)
{
unsigned int endtime;
int samps;
if (!sound_started || (snd_blocked > 0))
return;
SNDDMA_LockBuffer ();
if (! shm->buffer)
return;
// Updates DMA time
GetSoundtime();
// check to make sure that we haven't overshot
if (paintedtime < soundtime)
{
// Con_Printf ("S_Update_ : overflow\n");
paintedtime = soundtime;
}
// mix ahead of current position
endtime = soundtime + (unsigned int)(_snd_mixahead.value * shm->speed);
samps = shm->samples >> (shm->channels - 1);
endtime = q_min(endtime, (unsigned int)(soundtime + samps));
S_PaintChannels (endtime);
SNDDMA_Submit ();
}
/*
========================
Z_Realloc
========================
*/
void *Z_Realloc(void *ptr, int size)
{
int old_size;
void *old_ptr;
memblock_t *block;
if (!ptr)
return Z_Malloc (size);
block = (memblock_t *) ((byte *) ptr - sizeof (memblock_t));
if (block->id != ZONEID)
Sys_Error ("Z_Realloc: realloced a pointer without ZONEID");
if (block->tag == 0)
Sys_Error ("Z_Realloc: realloced a freed pointer");
old_size = block->size;
old_size -= (4 + (int)sizeof(memblock_t)); /* see Z_TagMalloc() */
old_ptr = ptr;
Z_Free (ptr);
ptr = Z_TagMalloc (size, 1);
if (!ptr)
Sys_Error ("Z_Realloc: failed on allocation of %i bytes", size);
if (ptr != old_ptr)
memmove (ptr, old_ptr, q_min(old_size, size));
if (old_size < size)
memset ((byte *)ptr + old_size, 0, size - old_size);
return ptr;
}
void *Z_Realloc (void *ptr, int size, int zone_id)
{
int old_size;
void *old_ptr;
memblock_t *block;
if (!ptr)
return Z_Malloc (size, zone_id);
block = (memblock_t *) ((byte *) ptr - sizeof (memblock_t));
if (block->id != ZONEID && block->id != ZONEID2)
Sys_Error ("%s: realloced a pointer without ZONEID", __thisfunc__);
if (block->tag == 0)
Sys_Error ("%s: realloced a freed pointer", __thisfunc__);
old_size = block->size;
old_ptr = ptr;
Z_Free (ptr);
ptr = Z_TagMalloc (zone_id, size, 1);
if (!ptr)
Sys_Error ("%s: failed on allocation of %i bytes", __thisfunc__, size);
if (ptr != old_ptr)
memmove (ptr, old_ptr, q_min(old_size, size));
return ptr;
}
interval mul_ii(interval x, interval y) /* [x] * [y] */
{ interval res;
double h;
if (x.INF >=0) { /* 0 <= [x] */
if (y.INF >=0) { /* 0 <= [y] */
h=x.INF*y.INF;
res.INF=(h==0 ? 0 : q_pred(h));
} else { /* [y] <= 0 or 0 \in [y] */
h=x.SUP*y.INF;
res.INF=(x.SUP==0 ? 0 : q_pred(h));
}
if (y.SUP <=0) { /* [y] <= 0 */
h=x.INF*y.SUP;
res.SUP=(h==0 ? 0 : q_succ(h));
} else { /* 0 <= [y] or 0 \in [y] */
h=x.SUP*y.SUP;
res.SUP=(x.SUP==0 ? 0 : q_succ(h));
}
} else if (x.SUP<=0) { /* [x] <= 0 */
if (y.SUP<=0) { /* [y] <= 0 */
h=x.SUP*y.SUP;
res.INF=(h==0 ? 0 : q_pred(h));
} else /* 0 <= [y] or 0 \in [y] */
res.INF=q_pred(x.INF*y.SUP);
if (y.INF>=0) { /* 0 <= [y] */
h=x.SUP*y.INF;
res.SUP=(h==0 ? 0 : q_succ(h));
} else /* [y] <= 0 or 0 \in [y] */
res.SUP=q_succ(x.INF*y.INF);
} else { /* 0 \in [x] */
if (y.INF>=0) { /* 0 <= [y] */
res.INF=q_pred(x.INF*y.SUP);
res.SUP=q_succ(x.SUP*y.SUP);
} else if (y.SUP<=0) { /* [y] <= 0 */
res.INF=q_pred(x.SUP*y.INF);
res.SUP=q_succ(x.INF*y.INF);
} else { /* 0 \in [x], 0 \in [y] */
res.INF=q_pred( q_min(x.INF*y.SUP, x.SUP*y.INF) );
res.SUP=q_succ( q_max(x.INF*y.INF, x.SUP*y.SUP) );
}
}
return res;
}
/*
===============
R_ParticleTextureLookup -- johnfitz -- generate nice antialiased 32x32 circle for particles
===============
*/
int R_ParticleTextureLookup (int x, int y, int sharpness)
{
int r; //distance from point x,y to circle origin, squared
int a; //alpha value to return
x -= 16;
y -= 16;
r = x * x + y * y;
r = r > 255 ? 255 : r;
a = sharpness * (255 - r);
a = q_min(a,255);
return a;
}
bool ChainIkSolverPos_LMA_JL_Mimic::obeysLimits(const KDL::JntArray &q_out)
{
bool obeys_limits = true;
for(size_t i = 0; i < chain.getNrOfJoints(); i++)
{
if( (q_out(i) < (q_min(i)-0.0001)) || (q_out(i) > (q_max(i)+0.0001)) )
{
// One element of solution is not within limits
obeys_limits = false;
break;
}
}
return obeys_limits;
}
/*
================
IN_ApplyDeadzone
in: raw joystick axis values converted to floats in +-1
out: applies a circular deadzone and clamps the magnitude at 1
(my 360 controller is slightly non-circular and the stick travels further on the diagonals)
deadzone is expected to satisfy 0 < deadzone < 1
from https://github.com/jeremiah-sypult/Quakespasm-Rift
and adapted from http://www.third-helix.com/2013/04/12/doing-thumbstick-dead-zones-right.html
================
*/
static joyaxis_t IN_ApplyDeadzone(joyaxis_t axis, float deadzone)
{
joyaxis_t result = {0};
vec_t magnitude = IN_AxisMagnitude(axis);
if ( magnitude > deadzone ) {
const vec_t new_magnitude = q_min(1.0, (magnitude - deadzone) / (1.0 - deadzone));
const vec_t scale = new_magnitude / magnitude;
result.x = axis.x * scale;
result.y = axis.y * scale;
}
return result;
}
/*
===============
Host_SavegameComment
Writes a SAVEGAME_COMMENT_LENGTH character comment describing the current
===============
*/
void Host_SavegameComment (char *text)
{
int i;
char kills[20];
for (i = 0; i < SAVEGAME_COMMENT_LENGTH; i++)
text[i] = ' ';
memcpy (text, cl.levelname, q_min(strlen(cl.levelname),22)); //johnfitz -- only copy 22 chars.
sprintf (kills,"kills:%3i/%3i", cl.stats[STAT_MONSTERS], cl.stats[STAT_TOTALMONSTERS]);
memcpy (text+22, kills, strlen(kills));
// convert space to _ to make stdio happy
for (i = 0; i < SAVEGAME_COMMENT_LENGTH; i++)
{
if (text[i] == ' ')
text[i] = '_';
}
text[SAVEGAME_COMMENT_LENGTH] = '\0';
}
TorqueEstimationTree::TorqueEstimationTree(KDL::Tree& icub_kdl,
std::vector< kdl_format_io::FTSensorData > ft_sensors,
std::vector< std::string > ft_serialization,
std::string fixed_link, unsigned int verbose)
{
yarp::sig::Vector q_max(icub_kdl.getNrOfJoints(),1000.0);
yarp::sig::Vector q_min(icub_kdl.getNrOfJoints(),-1000.0);
KDL::CoDyCo::TreeSerialization serial(icub_kdl);
std::vector<std::string> dof_serialization;
for(int i = 0; i < serial.getNrOfDOFs(); i++ )
{
dof_serialization.push_back(serial.getDOFName(i));
}
TorqueEstimationConstructor(icub_kdl,ft_sensors,dof_serialization,ft_serialization,q_min,q_max,fixed_link,verbose);
}
/*
==================
V_CalcRefdef
==================
*/
void V_CalcRefdef (void)
{
entity_t *ent, *view;
int i;
vec3_t forward, right, up;
vec3_t angles;
float bob;
static float oldz = 0;
static vec3_t punch = {0,0,0}; //johnfitz -- v_gunkick
float delta; //johnfitz -- v_gunkick
V_DriftPitch ();
// ent is the player model (visible when out of body)
ent = &cl_entities[cl.viewentity];
// view is the weapon model (only visible from inside body)
view = &cl.viewent;
// transform the view offset by the model's matrix to get the offset from
// model origin for the view
ent->angles[YAW] = cl.viewangles[YAW]; // the model should face the view dir
ent->angles[PITCH] = -cl.viewangles[PITCH]; // the model should face the view dir
bob = V_CalcBob ();
// refresh position
VectorCopy (ent->origin, r_refdef.vieworg);
r_refdef.vieworg[2] += cl.viewheight + bob;
// never let it sit exactly on a node line, because a water plane can
// dissapear when viewed with the eye exactly on it.
// the server protocol only specifies to 1/16 pixel, so add 1/32 in each axis
r_refdef.vieworg[0] += 1.0/32;
r_refdef.vieworg[1] += 1.0/32;
r_refdef.vieworg[2] += 1.0/32;
VectorCopy (cl.viewangles, r_refdef.viewangles);
V_CalcViewRoll ();
V_AddIdle ();
// offsets
angles[PITCH] = -ent->angles[PITCH]; // because entity pitches are actually backward
angles[YAW] = ent->angles[YAW];
angles[ROLL] = ent->angles[ROLL];
AngleVectors (angles, forward, right, up);
if (cl.maxclients <= 1) //johnfitz -- moved cheat-protection here from V_RenderView
for (i=0 ; i<3 ; i++)
r_refdef.vieworg[i] += scr_ofsx.value*forward[i] + scr_ofsy.value*right[i] + scr_ofsz.value*up[i];
V_BoundOffsets ();
// set up gun position
VectorCopy (cl.viewangles, view->angles);
CalcGunAngle ();
VectorCopy (ent->origin, view->origin);
view->origin[2] += cl.viewheight;
for (i=0 ; i<3 ; i++)
view->origin[i] += forward[i]*bob*0.4;
view->origin[2] += bob;
//johnfitz -- removed all gun position fudging code (was used to keep gun from getting covered by sbar)
//MarkV -- restored this with r_viewmodel_quake cvar
if (r_viewmodel_quake.value)
{
if (scr_viewsize.value == 110)
view->origin[2] += 1;
else if (scr_viewsize.value == 100)
view->origin[2] += 2;
else if (scr_viewsize.value == 90)
view->origin[2] += 1;
else if (scr_viewsize.value == 80)
view->origin[2] += 0.5;
}
view->model = cl.model_precache[cl.stats[STAT_WEAPON]];
view->frame = cl.stats[STAT_WEAPONFRAME];
view->colormap = vid.colormap;
//johnfitz -- v_gunkick
if (v_gunkick.value == 1) //original quake kick
VectorAdd (r_refdef.viewangles, cl.punchangle, r_refdef.viewangles);
if (v_gunkick.value == 2) //lerped kick
{
for (i=0; i<3; i++)
if (punch[i] != v_punchangles[0][i])
{
//speed determined by how far we need to lerp in 1/10th of a second
delta = (v_punchangles[0][i]-v_punchangles[1][i]) * host_frametime * 10;
if (delta > 0)
punch[i] = q_min(punch[i]+delta, v_punchangles[0][i]);
else if (delta < 0)
punch[i] = q_max(punch[i]+delta, v_punchangles[0][i]);
}
VectorAdd (r_refdef.viewangles, punch, r_refdef.viewangles);
}
//johnfitz
// smooth out stair step ups
if (!noclip_anglehack && cl.onground && ent->origin[2] - oldz > 0) //johnfitz -- added exception for noclip
//FIXME: noclip_anglehack is set on the server, so in a nonlocal game this won't work.
{
float steptime;
steptime = cl.time - cl.oldtime;
if (steptime < 0)
//FIXME I_Error ("steptime < 0");
steptime = 0;
oldz += steptime * 80;
if (oldz > ent->origin[2])
oldz = ent->origin[2];
if (ent->origin[2] - oldz > 12)
oldz = ent->origin[2] - 12;
r_refdef.vieworg[2] += oldz - ent->origin[2];
view->origin[2] += oldz - ent->origin[2];
}
else
oldz = ent->origin[2];
if (chase_active.value)
Chase_UpdateForDrawing (); //johnfitz
}
/*
================
GL_SetCanvas -- johnfitz -- support various canvas types
================
*/
void GL_SetCanvas (canvastype newcanvas)
{
extern vrect_t scr_vrect;
float s;
int lines;
if (newcanvas == currentcanvas)
return;
currentcanvas = newcanvas;
glMatrixMode(GL_PROJECTION);
glLoadIdentity ();
switch(newcanvas)
{
case CANVAS_DEFAULT:
glOrtho (0, glwidth, glheight, 0, -99999, 99999);
glViewport (glx, gly, glwidth, glheight);
break;
case CANVAS_CONSOLE:
lines = vid.conheight - (scr_con_current * vid.conheight / glheight);
glOrtho (0, vid.conwidth, vid.conheight + lines, lines, -99999, 99999);
glViewport (glx, gly, glwidth, glheight);
break;
case CANVAS_MENU:
s = q_min((float)glwidth / 320.0, (float)glheight / 200.0);
s = CLAMP (1.0, scr_menuscale.value, s);
// ericw -- doubled width to 640 to accommodate long keybindings
glOrtho (0, 640, 200, 0, -99999, 99999);
glViewport (glx + (glwidth - 320*s) / 2, gly + (glheight - 200*s) / 2, 640*s, 200*s);
break;
case CANVAS_SBAR:
s = CLAMP (1.0, scr_sbarscale.value, (float)glwidth / 320.0);
if (cl.gametype == GAME_DEATHMATCH)
{
glOrtho (0, glwidth / s, 48, 0, -99999, 99999);
glViewport (glx, gly, glwidth, 48*s);
}
else
{
glOrtho (0, 320, 48, 0, -99999, 99999);
glViewport (glx + (glwidth - 320*s) / 2, gly, 320*s, 48*s);
}
break;
case CANVAS_WARPIMAGE:
glOrtho (0, 128, 0, 128, -99999, 99999);
glViewport (glx, gly+glheight-gl_warpimagesize, gl_warpimagesize, gl_warpimagesize);
break;
case CANVAS_CROSSHAIR: //0,0 is center of viewport
s = CLAMP (1.0, scr_crosshairscale.value, 10.0);
glOrtho (scr_vrect.width/-2/s, scr_vrect.width/2/s, scr_vrect.height/2/s, scr_vrect.height/-2/s, -99999, 99999);
glViewport (scr_vrect.x, glheight - scr_vrect.y - scr_vrect.height, scr_vrect.width & ~1, scr_vrect.height & ~1);
break;
case CANVAS_BOTTOMLEFT: //used by devstats
s = (float)glwidth/vid.conwidth; //use console scale
glOrtho (0, 320, 200, 0, -99999, 99999);
glViewport (glx, gly, 320*s, 200*s);
break;
case CANVAS_BOTTOMRIGHT: //used by fps/clock
s = (float)glwidth/vid.conwidth; //use console scale
glOrtho (0, 320, 200, 0, -99999, 99999);
glViewport (glx+glwidth-320*s, gly, 320*s, 200*s);
break;
case CANVAS_TOPRIGHT: //used by disc
s = 1;
glOrtho (0, 320, 200, 0, -99999, 99999);
glViewport (glx+glwidth-320*s, gly+glheight-200*s, 320*s, 200*s);
break;
default:
Sys_Error ("GL_SetCanvas: bad canvas type");
}
glMatrixMode(GL_MODELVIEW);
glLoadIdentity ();
}
/*
================
GLMesh_LoadVertexBuffer
Upload the given alias model's mesh to a VBO
Original code by MH from RMQEngine
================
*/
static void GLMesh_LoadVertexBuffer (qmodel_t *m, const aliashdr_t *hdr)
{
int totalvbosize = 0;
int remaining_size;
int copy_offset;
const aliasmesh_t *desc;
const short *indexes;
const trivertx_t *trivertexes;
byte *vbodata;
int f;
VkResult err;
// count the sizes we need
// ericw -- RMQEngine stored these vbo*ofs values in aliashdr_t, but we must not
// mutate Mod_Extradata since it might be reloaded from disk, so I moved them to qmodel_t
// (test case: roman1.bsp from arwop, 64mb heap)
m->vboindexofs = 0;
m->vboxyzofs = 0;
totalvbosize += (hdr->numposes * hdr->numverts_vbo * sizeof (meshxyz_t)); // ericw -- what RMQEngine called nummeshframes is called numposes in QuakeSpasm
m->vbostofs = totalvbosize;
totalvbosize += (hdr->numverts_vbo * sizeof (meshst_t));
if (!hdr->numindexes) return;
if (!totalvbosize) return;
// grab the pointers to data in the extradata
desc = (aliasmesh_t *) ((byte *) hdr + hdr->meshdesc);
indexes = (short *) ((byte *) hdr + hdr->indexes);
trivertexes = (trivertx_t *) ((byte *)hdr + hdr->vertexes);
{
const int totalindexsize = hdr->numindexes * sizeof (unsigned short);
// Allocate index buffer & upload to GPU
VkBufferCreateInfo buffer_create_info;
memset(&buffer_create_info, 0, sizeof(buffer_create_info));
buffer_create_info.sType = VK_STRUCTURE_TYPE_BUFFER_CREATE_INFO;
buffer_create_info.size = totalindexsize;
buffer_create_info.usage = VK_BUFFER_USAGE_INDEX_BUFFER_BIT | VK_BUFFER_USAGE_TRANSFER_DST_BIT;
err = vkCreateBuffer(vulkan_globals.device, &buffer_create_info, NULL, &m->index_buffer);
if (err != VK_SUCCESS)
Sys_Error("vkCreateBuffer failed");
GL_SetObjectName((uint64_t)m->index_buffer, VK_DEBUG_REPORT_OBJECT_TYPE_BUFFER_EXT, m->name);
VkMemoryRequirements memory_requirements;
vkGetBufferMemoryRequirements(vulkan_globals.device, m->index_buffer, &memory_requirements);
uint32_t memory_type_index = GL_MemoryTypeFromProperties(memory_requirements.memoryTypeBits, VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT, 0);
VkDeviceSize heap_size = INDEX_HEAP_SIZE_MB * (VkDeviceSize)1024 * (VkDeviceSize)1024;
VkDeviceSize aligned_offset = GL_AllocateFromHeaps(GEOMETRY_MAX_HEAPS, index_buffer_heaps, heap_size, memory_type_index, memory_requirements.size,
memory_requirements.alignment, &m->index_heap, &m->index_heap_node, &num_vulkan_mesh_allocations, "Index Buffers");
err = vkBindBufferMemory(vulkan_globals.device, m->index_buffer, m->index_heap->memory, aligned_offset);
if (err != VK_SUCCESS)
Sys_Error("vkBindBufferMemory failed");
remaining_size = totalindexsize;
copy_offset = 0;
while (remaining_size > 0)
{
const int size_to_copy = q_min(remaining_size, STAGING_BUFFER_SIZE_KB * 1024);
VkBuffer staging_buffer;
VkCommandBuffer command_buffer;
int staging_offset;
unsigned char * staging_memory = R_StagingAllocate(size_to_copy, 1, &command_buffer, &staging_buffer, &staging_offset);
memcpy(staging_memory, (byte*)indexes + copy_offset, size_to_copy);
VkBufferCopy region;
region.srcOffset = staging_offset;
region.dstOffset = copy_offset;
region.size = size_to_copy;
vkCmdCopyBuffer(command_buffer, staging_buffer, m->index_buffer, 1, ®ion);
copy_offset += size_to_copy;
remaining_size -= size_to_copy;
}
}
// create the vertex buffer (empty)
vbodata = (byte *) malloc(totalvbosize);
memset(vbodata, 0, totalvbosize);
// fill in the vertices at the start of the buffer
for (f = 0; f < hdr->numposes; f++) // ericw -- what RMQEngine called nummeshframes is called numposes in QuakeSpasm
{
int v;
meshxyz_t *xyz = (meshxyz_t *) (vbodata + (f * hdr->numverts_vbo * sizeof (meshxyz_t)));
const trivertx_t *tv = trivertexes + (hdr->numverts * f);
for (v = 0; v < hdr->numverts_vbo; v++)
{
trivertx_t trivert = tv[desc[v].vertindex];
xyz[v].xyz[0] = trivert.v[0];
xyz[v].xyz[1] = trivert.v[1];
xyz[v].xyz[2] = trivert.v[2];
xyz[v].xyz[3] = 1; // need w 1 for 4 byte vertex compression
// map the normal coordinates in [-1..1] to [-127..127] and store in an unsigned char.
// this introduces some error (less than 0.004), but the normals were very coarse
// to begin with
xyz[v].normal[0] = 127 * r_avertexnormals[trivert.lightnormalindex][0];
xyz[v].normal[1] = 127 * r_avertexnormals[trivert.lightnormalindex][1];
xyz[v].normal[2] = 127 * r_avertexnormals[trivert.lightnormalindex][2];
xyz[v].normal[3] = 0; // unused; for 4-byte alignment
}
}
// fill in the ST coords at the end of the buffer
{
meshst_t *st;
float hscale, vscale;
//johnfitz -- padded skins
hscale = (float)hdr->skinwidth/(float)TexMgr_PadConditional(hdr->skinwidth);
vscale = (float)hdr->skinheight/(float)TexMgr_PadConditional(hdr->skinheight);
//johnfitz
st = (meshst_t *) (vbodata + m->vbostofs);
for (f = 0; f < hdr->numverts_vbo; f++)
{
st[f].st[0] = hscale * ((float) desc[f].st[0] + 0.5f) / (float) hdr->skinwidth;
st[f].st[1] = vscale * ((float) desc[f].st[1] + 0.5f) / (float) hdr->skinheight;
}
}
// Allocate vertex buffer & upload to GPU
{
VkBufferCreateInfo buffer_create_info;
memset(&buffer_create_info, 0, sizeof(buffer_create_info));
buffer_create_info.sType = VK_STRUCTURE_TYPE_BUFFER_CREATE_INFO;
buffer_create_info.size = totalvbosize;
buffer_create_info.usage = VK_BUFFER_USAGE_VERTEX_BUFFER_BIT | VK_BUFFER_USAGE_TRANSFER_DST_BIT;
err = vkCreateBuffer(vulkan_globals.device, &buffer_create_info, NULL, &m->vertex_buffer);
if (err != VK_SUCCESS)
Sys_Error("vkCreateBuffer failed");
GL_SetObjectName((uint64_t)m->vertex_buffer, VK_DEBUG_REPORT_OBJECT_TYPE_BUFFER_EXT, m->name);
VkMemoryRequirements memory_requirements;
vkGetBufferMemoryRequirements(vulkan_globals.device, m->vertex_buffer, &memory_requirements);
uint32_t memory_type_index = GL_MemoryTypeFromProperties(memory_requirements.memoryTypeBits, VK_MEMORY_PROPERTY_DEVICE_LOCAL_BIT, 0);
VkDeviceSize heap_size = VERTEX_HEAP_SIZE_MB * (VkDeviceSize)1024 * (VkDeviceSize)1024;
VkDeviceSize aligned_offset = GL_AllocateFromHeaps(GEOMETRY_MAX_HEAPS, vertex_buffer_heaps, heap_size, memory_type_index, memory_requirements.size,
memory_requirements.alignment, &m->vertex_heap, &m->vertex_heap_node, &num_vulkan_mesh_allocations, "Vertex Buffers");
err = vkBindBufferMemory(vulkan_globals.device, m->vertex_buffer, m->vertex_heap->memory, aligned_offset);
if (err != VK_SUCCESS)
Sys_Error("vkBindBufferMemory failed");
remaining_size = totalvbosize;
copy_offset = 0;
while (remaining_size > 0)
{
const int size_to_copy = q_min(remaining_size, STAGING_BUFFER_SIZE_KB * 1024);
VkBuffer staging_buffer;
VkCommandBuffer command_buffer;
int staging_offset;
unsigned char * staging_memory = R_StagingAllocate(size_to_copy, 1, &command_buffer, &staging_buffer, &staging_offset);
memcpy(staging_memory, (byte*)vbodata + copy_offset, size_to_copy);
VkBufferCopy region;
region.srcOffset = staging_offset;
region.dstOffset = copy_offset;
region.size = size_to_copy;
vkCmdCopyBuffer(command_buffer, staging_buffer, m->vertex_buffer, 1, ®ion);
copy_offset += size_to_copy;
remaining_size -= size_to_copy;
}
}
free (vbodata);
}
bool KDLRobotModel::init(std::string robot_description, std::vector<std::string> &planning_joints)
{
urdf_ = boost::shared_ptr<urdf::Model>(new urdf::Model());
if (!urdf_->initString(robot_description))
{
ROS_ERROR("Failed to parse the URDF.");
return false;
}
if (!kdl_parser::treeFromUrdfModel(*urdf_, ktree_))
{
ROS_ERROR("Failed to parse the kdl tree from robot description.");
return false;
}
std::vector<std::string> segments(planning_joints.size());
for(size_t j = 0; j < planning_joints.size(); ++j)
{
if(!leatherman::getSegmentOfJoint(ktree_, planning_joints[j], segments[j]))
{
ROS_ERROR("Failed to find kdl segment for '%s'.", planning_joints_[j].c_str());
return false;
}
}
/*
if(!leatherman::getChainTip(ktree_, segments, chain_root_name_, chain_tip_name_))
{
ROS_ERROR("Failed to find a valid chain tip link.");
return false;
}
*/
if(!ktree_.getChain(chain_root_name_, chain_tip_name_, kchain_))
{
ROS_ERROR("Failed to fetch the KDL chain for the robot. (root: %s, tip: %s)", chain_root_name_.c_str(), chain_tip_name_.c_str());
return false;
}
// check if our chain includes all planning joints
for(size_t i = 0; i < planning_joints.size(); ++i)
{
if(planning_joints[i].empty())
{
ROS_ERROR("Planning joint name is empty (index: %d).", int(i));
return false;
}
int index;
if(!leatherman::getJointIndex(kchain_, planning_joints[i], index))
{
ROS_ERROR("Failed to find '%s' in the kinematic chain. Maybe your chain root or tip joints are wrong? (%s, %s)", planning_joints[i].c_str(), chain_root_name_.c_str(), chain_tip_name_.c_str());
return false;
}
}
// joint limits
planning_joints_ = planning_joints;
if(!getJointLimits(planning_joints_, min_limits_, max_limits_, continuous_))
{
ROS_ERROR("Failed to get the joint limits.");
return false;
}
// FK solver
fk_solver_ = new KDL::ChainFkSolverPos_recursive(kchain_);
jnt_pos_in_.resize(kchain_.getNrOfJoints());
jnt_pos_out_.resize(kchain_.getNrOfJoints());
// IK solver
KDL::JntArray q_min(planning_joints_.size());
KDL::JntArray q_max(planning_joints_.size());
for(size_t i = 0; i < planning_joints_.size(); ++i)
{
q_min(i) = min_limits_[i];
q_max(i) = max_limits_[i];
}
ik_vel_solver_ = new KDL::ChainIkSolverVel_pinv(kchain_);
ik_solver_ = new KDL::ChainIkSolverPos_NR_JL(kchain_, q_min, q_max, *fk_solver_, *ik_vel_solver_, 200, 0.001);
// joint name -> index mapping
for(size_t i = 0; i < planning_joints_.size(); ++i)
joint_map_[planning_joints_[i]] = i;
// link name -> kdl index mapping
for(size_t i = 0; i < kchain_.getNrOfSegments(); ++i)
link_map_[kchain_.getSegment(i).getName()] = i;
initialized_ = true;
return true;
}
static
bool Init(
KDLRobotModel* model,
const std::string& robot_description,
const std::string& base_link,
const std::string& tip_link,
int free_angle)
{
ROS_INFO("Initialize KDL Robot Model");
if (!model->m_urdf.initString(robot_description)) {
ROS_ERROR("Failed to parse the URDF.");
return false;
}
ROS_INFO("Initialize Robot Model");
urdf::JointSpec world_joint;
world_joint.name = "map";
world_joint.origin = Eigen::Affine3d::Identity(); // IMPORTANT
world_joint.axis = Eigen::Vector3d::Zero();
world_joint.type = urdf::JointType::Floating;
if (!urdf::InitRobotModel(
&model->m_robot_model, &model->m_urdf, &world_joint))
{
ROS_ERROR("Failed to initialize Robot Model");
return false;
}
ROS_INFO("Initialize KDL tree");
if (!kdl_parser::treeFromUrdfModel(model->m_urdf, model->m_tree)) {
ROS_ERROR("Failed to parse the kdl tree from robot description.");
return false;
}
ROS_INFO("Initialize KDL chain (%s, %s)", base_link.c_str(), tip_link.c_str());
if (!model->m_tree.getChain(base_link, tip_link, model->m_chain)) {
ROS_ERROR("Failed to fetch the KDL chain for the robot. (root: %s, tip: %s)", base_link.c_str(), tip_link.c_str());
return false;
}
model->m_base_link = base_link;
model->m_tip_link = tip_link;
model->m_kinematics_link = GetLink(&model->m_robot_model, &base_link);
if (model->m_kinematics_link == NULL) {
return false; // this shouldn't happen if the chain initialized successfully
}
ROS_INFO("Gather joints in chain");
std::vector<std::string> planning_joints;
for (auto i = (unsigned)0; i < model->m_chain.getNrOfSegments(); ++i) {
auto& segment = model->m_chain.getSegment(i);
auto& child_joint_name = segment.getJoint().getName();
auto* joint = GetJoint(&model->m_robot_model, &child_joint_name);
if (GetVariableCount(joint) > 1) {
ROS_WARN("> 1 variable per joint");
return false;
}
if (GetVariableCount(joint) == 0) {
continue;
}
planning_joints.push_back(child_joint_name);
}
ROS_INFO("Initialize URDF Robot Model with planning joints = %s", to_string(planning_joints).c_str());
if (!urdf::Init(model, &model->m_robot_model, &planning_joints)) {
ROS_ERROR("Failed to initialize URDF Robot Model");
return false;
}
// do this after we've initialized the URDFRobotModel...
model->planning_link = GetLink(&model->m_robot_model, &tip_link);
if (model->planning_link == NULL) {
return false; // this shouldn't happen either
}
// FK solver
model->m_fk_solver = make_unique<KDL::ChainFkSolverPos_recursive>(model->m_chain);
// IK Velocity solver
model->m_ik_vel_solver = make_unique<KDL::ChainIkSolverVel_pinv>(model->m_chain);
// IK solver
KDL::JntArray q_min(model->jointVariableCount());
KDL::JntArray q_max(model->jointVariableCount());
for (size_t i = 0; i < model->jointVariableCount(); ++i) {
if (model->vprops[i].continuous) {
q_min(i) = -M_PI;
q_max(i) = M_PI;
} else {
q_min(i) = model->vprops[i].min_position;
q_max(i) = model->vprops[i].max_position;
}
}
model->m_max_iterations = 200;
model->m_kdl_eps = 0.001;
model->m_ik_solver = make_unique<KDL::ChainIkSolverPos_NR_JL>(
model->m_chain,
q_min,
q_max,
*model->m_fk_solver,
*model->m_ik_vel_solver,
model->m_max_iterations,
model->m_kdl_eps);
model->m_jnt_pos_in.resize(model->m_chain.getNrOfJoints());
model->m_jnt_pos_out.resize(model->m_chain.getNrOfJoints());
model->m_free_angle = free_angle;
model->m_search_discretization = 0.02;
model->m_timeout = 0.005;
return true;
}
/* Read up to len samples from p->Synth
* If needed, read some more MP3 data, decode them and synth them
* Place in buf[].
* Return number of samples read. */
static int mp3_decode(snd_stream_t *stream, byte *buf, int len)
{
mp3_priv_t *p = (mp3_priv_t *) stream->priv;
int donow, i, done = 0;
mad_fixed_t sample;
int chan, x;
do
{
x = (p->Synth.pcm.length - p->cursamp) * stream->info.channels;
donow = q_min(len, x);
i = 0;
while (i < donow)
{
for (chan = 0; chan < stream->info.channels; chan++)
{
sample = p->Synth.pcm.samples[chan][p->cursamp];
/* convert from fixed to short,
* write in host-endian format. */
if (sample <= -MAD_F_ONE)
sample = -0x7FFF;
else if (sample >= MAD_F_ONE)
sample = 0x7FFF;
else
sample >>= (MAD_F_FRACBITS + 1 - 16);
if (host_bigendian)
{
*buf++ = (sample >> 8) & 0xFF;
*buf++ = sample & 0xFF;
}
else /* assumed LITTLE_ENDIAN. */
{
*buf++ = sample & 0xFF;
*buf++ = (sample >> 8) & 0xFF;
}
i++;
}
p->cursamp++;
}
len -= donow;
done += donow;
if (len == 0)
break;
/* check whether input buffer needs a refill */
if (p->Stream.error == MAD_ERROR_BUFLEN)
{
if (mp3_inputdata(stream) == -1)
{
/* check feof() ?? */
Con_DPrintf("mp3 EOF\n");
break;
}
}
if (mad_frame_decode(&p->Frame, &p->Stream))
{
if (MAD_RECOVERABLE(p->Stream.error))
{
mp3_inputtag(stream);
continue;
}
else
{
if (p->Stream.error == MAD_ERROR_BUFLEN)
continue;
else
{
Con_Printf("MP3: unrecoverable frame level error (%s)\n",
mad_stream_errorstr(&p->Stream));
break;
}
}
}
p->FrameCount++;
mad_timer_add(&p->Timer, p->Frame.header.duration);
mad_synth_frame(&p->Synth, &p->Frame);
p->cursamp = 0;
} while (1);
bool unigripper_motoman_plant_t::IK_solver(const config_t& effector_config, space_point_t* computed_state, bool set_grasping, const space_point_t* seed_state, bool do_min)
{
double qx,qy,qz,qw;
double x,y,z;
effector_config.get_orientation().get(qx,qy,qz,qw);
effector_config.get_position(x,y,z);
KDL::Chain chain1;
my_tree->getChain("base_link","vacuum_volume",chain1);
KDL::JntArray q(chain1.getNrOfJoints());
KDL::JntArray q_out(chain1.getNrOfJoints());
KDL::JntArray q_min(chain1.getNrOfJoints());
KDL::JntArray q_max(chain1.getNrOfJoints());
std::vector< double > state_var;
if( seed_state != NULL )
state_space->copy_point_to_vector( seed_state, state_var );
else
state_space->copy_to_vector( state_var );
q(0) = state_var[0];
q(1) = state_var[1];
q(2) = state_var[2];
q(3) = state_var[3];
q(4) = state_var[4];
q(5) = state_var[5];
q(6) = state_var[6];
q(7) = state_var[7];
q(8) = state_var[15];
q_min(0) = state_space->get_bounds()[0]->get_lower_bound();
q_min(1) = state_space->get_bounds()[1]->get_lower_bound();
q_min(2) = state_space->get_bounds()[2]->get_lower_bound();
q_min(3) = state_space->get_bounds()[3]->get_lower_bound();
q_min(4) = state_space->get_bounds()[4]->get_lower_bound();
q_min(5) = state_space->get_bounds()[5]->get_lower_bound();
q_min(6) = state_space->get_bounds()[6]->get_lower_bound();
q_min(7) = state_space->get_bounds()[7]->get_lower_bound();
q_min(8) = state_space->get_bounds()[15]->get_lower_bound();
q_max(0) = state_space->get_bounds()[0]->get_upper_bound();
q_max(1) = state_space->get_bounds()[1]->get_upper_bound();
q_max(2) = state_space->get_bounds()[2]->get_upper_bound();
q_max(3) = state_space->get_bounds()[3]->get_upper_bound();
q_max(4) = state_space->get_bounds()[4]->get_upper_bound();
q_max(5) = state_space->get_bounds()[5]->get_upper_bound();
q_max(6) = state_space->get_bounds()[6]->get_upper_bound();
q_max(7) = state_space->get_bounds()[7]->get_upper_bound();
q_max(8) = state_space->get_bounds()[15]->get_upper_bound();
KDL::ChainFkSolverPos_recursive fk_solver(chain1);
KDL::ChainIkSolverVel_pinv ik_solver_vel(chain1);
KDL::ChainIkSolverPos_NR_JL ik_solver(chain1,q_min,q_max,fk_solver,ik_solver_vel,1000,1e-6);
KDL::Frame F(KDL::Rotation::Quaternion(qx,qy,qz,qw),KDL::Vector(x,y,z));
bool ik_res = (ik_solver.CartToJnt(q,F,q_out)>=0);
if(ik_res)
{
std::vector<double> state_vec;
state_vec = state_var;
state_vec[0] = q_out(0);
state_vec[1] = q_out(1);
state_vec[2] = q_out(2);
state_vec[3] = q_out(3);
state_vec[4] = q_out(4);
state_vec[5] = q_out(5);
state_vec[6] = q_out(6);
state_vec[7] = q_out(7);
state_vec[15] = q_out(8);
state_vec[16] = set_grasping ? (double)GRIPPER_CLOSED : GRIPPER_OPEN;
state_space->copy_vector_to_point( state_vec, computed_state );
}
return ik_res;
}
/*
=============
GLSLGamma_GammaCorrect
=============
*/
void GLSLGamma_GammaCorrect (void)
{
float smax, tmax;
if (!gl_glsl_gamma_able)
return;
if (vid_gamma.value == 1 && vid_contrast.value == 1)
return;
// create render-to-texture texture if needed
if (!r_gamma_texture)
{
glGenTextures (1, &r_gamma_texture);
glBindTexture (GL_TEXTURE_2D, r_gamma_texture);
r_gamma_texture_width = glwidth;
r_gamma_texture_height = glheight;
if (!gl_texture_NPOT)
{
r_gamma_texture_width = TexMgr_Pad(r_gamma_texture_width);
r_gamma_texture_height = TexMgr_Pad(r_gamma_texture_height);
}
glTexImage2D (GL_TEXTURE_2D, 0, GL_RGBA8, r_gamma_texture_width, r_gamma_texture_height, 0, GL_BGRA, GL_UNSIGNED_INT_8_8_8_8_REV, NULL);
glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MAG_FILTER, GL_NEAREST);
glTexParameteri (GL_TEXTURE_2D, GL_TEXTURE_MIN_FILTER, GL_NEAREST);
}
// create shader if needed
if (!r_gamma_program)
{
GLSLGamma_CreateShaders ();
if (!r_gamma_program)
{
Sys_Error("GLSLGamma_CreateShaders failed");
}
}
// copy the framebuffer to the texture
GL_DisableMultitexture();
glBindTexture (GL_TEXTURE_2D, r_gamma_texture);
glCopyTexSubImage2D (GL_TEXTURE_2D, 0, 0, 0, glx, gly, glwidth, glheight);
// draw the texture back to the framebuffer with a fragment shader
GL_UseProgramFunc (r_gamma_program);
GL_Uniform1fFunc (gammaLoc, vid_gamma.value);
GL_Uniform1fFunc (contrastLoc, q_min(2.0, q_max(1.0, vid_contrast.value)));
GL_Uniform1iFunc (textureLoc, 0); // use texture unit 0
glDisable (GL_ALPHA_TEST);
glDisable (GL_DEPTH_TEST);
glViewport (glx, gly, glwidth, glheight);
smax = glwidth/(float)r_gamma_texture_width;
tmax = glheight/(float)r_gamma_texture_height;
glBegin (GL_QUADS);
glTexCoord2f (0, 0);
glVertex2f (-1, -1);
glTexCoord2f (smax, 0);
glVertex2f (1, -1);
glTexCoord2f (smax, tmax);
glVertex2f (1, 1);
glTexCoord2f (0, tmax);
glVertex2f (-1, 1);
glEnd ();
GL_UseProgramFunc (0);
// clear cached binding
GL_ClearBindings ();
}
