void dJointSetPlane2DAngleParam( dxJoint *joint,
int parameter, dReal value )
{
dUASSERT( joint, "bad joint argument" );
checktype( joint, Plane2D );
dxJointPlane2D* joint2d = ( dxJointPlane2D* )( joint );
joint2d->motor_angle.set( parameter, value );
}
void dJointGetSliderAxis ( dJointID j, dVector3 result )
{
dxJointSlider* joint = ( dxJointSlider* ) j;
dUASSERT ( joint, "bad joint argument" );
dUASSERT ( result, "bad result argument" );
checktype ( joint, Slider );
getAxis ( joint, result, joint->axis1 );
}
void dJointGetPUAxis3( dJointID j, dVector3 result )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, PU );
getAxis( joint, result, joint->axisP1 );
}
static int
optboolean(lua_State *L, int narg, int def)
{
if (lua_isnoneornil(L, narg))
return def;
checktype (L, narg, LUA_TBOOLEAN, "boolean or nil");
return (int)lua_toboolean(L, narg);
}
void dJointGetHingeAxis( dJointID j, dVector3 result )
{
dxJointHinge* joint = ( dxJointHinge* )j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, Hinge );
getAxis( joint, result, joint->axis1 );
}
void dJointSetHingeAxis( dJointID j, dReal x, dReal y, dReal z )
{
dxJointHinge* joint = ( dxJointHinge* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge );
setAxes( joint, x, y, z, joint->axis1, joint->axis2 );
joint->computeInitialRelativeRotation();
}
void dJointGetPRAxis2( dJointID j, dVector3 result )
{
dxJointPR* joint = ( dxJointPR* ) j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, PR );
getAxis( joint, result, joint->axisR1 );
}
void dJointSetScrewAnchor( dJointID j, dReal x, dReal y, dReal z )
{
dxJointScrew* joint = ( dxJointScrew* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Screw );
setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
joint->computeInitialRelativeRotation();
}
void dJointSetPRAxis2( dJointID j, dReal x, dReal y, dReal z )
{
dxJointPR* joint = ( dxJointPR* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PR );
setAxes( joint, x, y, z, joint->axisR1, joint->axisR2 );
joint->computeInitialRelativeRotation();
}
dReal dJointGetHinge2Angle1( dJointID j )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
if ( joint->node[0].body ) return joint->measureAngle();
else return 0;
}
void dJointSetHinge2Anchor( dJointID j, dReal x, dReal y, dReal z )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Hinge2 );
setAnchors( joint, x, y, z, joint->anchor1, joint->anchor2 );
joint->makeV1andV2();
}
dReal dJointGetAMotorAngle( dJointID j, int anum )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 3 ) anum = 3;
return joint->angle[anum];
}
int dJointGetAMotorAxisRel( dJointID j, int anum )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
return joint->rel[anum];
}
void dJointGetAMotorAxis( dJointID j, int anum, dVector3 result )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint && anum >= 0 && anum < 3 );
checktype( joint, AMotor );
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
// If we're in Euler mode, joint->axis[1] doesn't
// have anything sensible in it. So don't just return
// that, find the actual effective axis.
// Likewise, the actual axis of rotation for the
// the other axes is different from what's stored.
if ( joint->mode == dAMotorEuler ) {
dVector3 axes[3];
joint->computeGlobalAxes(axes);
if (anum == 1) {
result[0]=axes[1][0];
result[1]=axes[1][1];
result[2]=axes[1][2];
} else if (anum == 0) {
// This won't be unit length in general,
// but it's what's used in getInfo2
// This may be why things freak out as
// the body-relative axes get close to each other.
dCalcVectorCross3( result, axes[1], axes[2] );
} else if (anum == 2) {
// Same problem as above.
dCalcVectorCross3( result, axes[0], axes[1] );
}
} else if ( joint->rel[anum] > 0 ) {
if ( joint->rel[anum] == 1 )
{
dMultiply0_331( result, joint->node[0].body->posr.R, joint->axis[anum] );
}
else
{
if ( joint->node[1].body ) // jds
{
dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis[anum] );
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
result[3] = joint->axis[anum][3];
}
}
}
else
{
result[0] = joint->axis[anum][0];
result[1] = joint->axis[anum][1];
result[2] = joint->axis[anum][2];
}
}
void dJointSetLMotorNumAxes( dJointID j, int num )
{
dxJointLMotor* joint = ( dxJointLMotor* )j;
dAASSERT( joint && num >= 0 && num <= 3 );
checktype( joint, LMotor );
if ( num < 0 ) num = 0;
if ( num > 3 ) num = 3;
joint->num = num;
}
void dJointPlanarSetAnchor(dJointID joint, dVector3 anchor) {
dUASSERT( joint, "bad joint argument" );
checktype( joint, Plane2D );
dxPlanarJoint* planarJoint = (dxPlanarJoint*) joint;
dCopyVector3(planarJoint->anchor, anchor);
planarJoint->updatePlane();
}
/**
* Locking routine.
* @param type: as passed by user.
* @param lock: as passed by user.
* @param func: caller location.
* @param file: caller location.
* @param line: caller location.
* @param tryfunc: the pthread_mutex_trylock or similar function.
* @param timedfunc: the pthread_mutex_timedlock or similar function.
* Uses absolute timeout value.
* @param arg: what to pass to tryfunc and timedlock.
* @param exclusive: if lock must be exclusive (only one allowed).
* @param getwr: if attempts to get writelock (or readlock) for rwlocks.
*/
static void
checklock_lockit(enum check_lock_type type, struct checked_lock* lock,
const char* func, const char* file, int line,
int (*tryfunc)(void*), int (*timedfunc)(void*, struct timespec*),
void* arg, int exclusive, int getwr)
{
int err;
int contend = 0;
struct thr_check *thr = (struct thr_check*)pthread_getspecific(
thr_debug_key);
checktype(type, lock, func, file, line);
if(!thr) lock_error(lock, func, file, line, "no thread info");
acquire_locklock(lock, func, file, line);
lock->wait_count ++;
thr->waiting = lock;
if(exclusive && lock->hold_count > 0 && lock->holder == thr)
lock_error(lock, func, file, line, "thread already owns lock");
if(type==check_lock_rwlock && getwr && lock->writeholder == thr)
lock_error(lock, func, file, line, "thread already has wrlock");
LOCKRET(pthread_mutex_unlock(&lock->lock));
/* first try; if busy increase contention counter */
if((err=tryfunc(arg))) {
struct timespec to;
if(err != EBUSY) log_err("trylock: %s", strerror(err));
to.tv_sec = time(NULL) + CHECK_LOCK_TIMEOUT;
to.tv_nsec = 0;
if((err=timedfunc(arg, &to))) {
if(err == ETIMEDOUT)
lock_error(lock, func, file, line,
"timeout possible deadlock");
log_err("timedlock: %s", strerror(err));
}
contend ++;
}
/* got the lock */
acquire_locklock(lock, func, file, line);
lock->contention_count += contend;
lock->history_count++;
if(exclusive && lock->hold_count > 0)
lock_error(lock, func, file, line, "got nonexclusive lock");
if(type==check_lock_rwlock && getwr && lock->writeholder)
lock_error(lock, func, file, line, "got nonexclusive wrlock");
if(type==check_lock_rwlock && getwr)
lock->writeholder = thr;
/* check the memory areas for unauthorized changes,
* between last unlock time and current lock time.
* we check while holding the lock (threadsafe).
*/
if(getwr || exclusive)
prot_check(lock, func, file, line);
finish_acquire_lock(thr, lock, func, file, line);
LOCKRET(pthread_mutex_unlock(&lock->lock));
}
int
dolocal(FILE *ofd, char *pre, int dowhat, int p, char *s)
{ int h, j, k=0; extern int nr_errs;
Ordered *walk;
Symbol *sp;
char buf[64], buf2[128], buf3[128];
if (dowhat == INIV)
{ /* initialize in order of declaration */
for (walk = all_names; walk; walk = walk->next)
{ sp = walk->entry;
if (sp->context
&& !sp->owner
&& strcmp(s, sp->context->name) == 0)
{ checktype(sp, s); /* fall through */
if (!(sp->hidden&16))
{ sprintf(buf, "((P%d *)pptr(h))->", p);
do_var(ofd, dowhat, buf, sp, "", " = ", ";\n");
}
k++;
} }
} else
{ for (j = 0; j < 8; j++)
for (h = 0; h <= 1; h++)
for (walk = all_names; walk; walk = walk->next)
{ sp = walk->entry;
if (sp->context
&& !sp->owner
&& sp->type == Types[j]
&& ((h == 0 && sp->nel == 1) || (h == 1 && sp->nel > 1))
&& strcmp(s, sp->context->name) == 0)
{ switch (dowhat) {
case LOGV:
if (sp->type == CHAN
&& verbose == 0)
break;
sprintf(buf, "%s%s:", pre, s);
{ sprintf(buf2, "\", ((P%d *)pptr(h))->", p);
sprintf(buf3, ");\n");
}
do_var(ofd, dowhat, "", sp, buf, buf2, buf3);
break;
case PUTV:
sprintf(buf, "((P%d *)pptr(h))->", p);
do_var(ofd, dowhat, buf, sp, "", " = ", ";\n");
k++;
break;
}
if (strcmp(s, ":never:") == 0)
{ printf("error: %s defines local %s\n",
s, sp->name);
nr_errs++;
} } } }
return k;
}
void dJointSetBallAnchor2( dJointID j, dReal x, dReal y, dReal z )
{
dxJointBall* joint = ( dxJointBall* )j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, Ball );
joint->anchor2[0] = x;
joint->anchor2[1] = y;
joint->anchor2[2] = z;
joint->anchor2[3] = 0;
}
dReal dJointGetPUAngle2( dJointID j )
{
dxJointUniversal* joint = ( dxJointUniversal* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->flags & dJOINT_REVERSE )
return joint->getAngle1();
else
return joint->getAngle2();
}
void dJointGetPUAngles( dJointID j, dReal *angle1, dReal *angle2 )
{
dxJointUniversal* joint = ( dxJointUniversal* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->flags & dJOINT_REVERSE )
joint->getAngles( angle2, angle1 );
else
joint->getAngles( angle1, angle2 );
}
void dJointSetPUAxis3( dJointID j, dReal x, dReal y, dReal z )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
setAxes( joint, x, y, z, joint->axisP1, 0 );
joint->computeInitialRelativeRotations();
}
void dJointGetScrewAnchor2( dJointID j, dVector3 result )
{
dxJointScrew* joint = ( dxJointScrew* )j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, Screw );
if ( joint->flags & dJOINT_REVERSE )
getAnchor( joint, result, joint->anchor1 );
else
getAnchor2( joint, result, joint->anchor2 );
}
static int WritePack(lua_State *L)
{
hostmem_t* hostmem = checkhostmem(L, 1, NULL);
size_t offset = luaL_checkinteger(L, 2);
int type = checktype(L, 3);
size_t size = hostmem->size - offset;
if(offset >= hostmem->size)
return luaL_error(L, errstring(ERR_BOUNDARIES));
checkdata(L, 4, type, hostmem->ptr + offset, size);
return 0;
}
void dJointSetAMotorParam( dJointID j, int parameter, dReal value )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint );
checktype( joint, AMotor );
int anum = parameter >> 8;
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
parameter &= 0xff;
joint->limot[anum].set( parameter, value );
}
void dJointGetHinge2Axis2( dJointID j, dVector3 result )
{
dxJointHinge2* joint = ( dxJointHinge2* )j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, Hinge2 );
if ( joint->node[1].body )
{
dMultiply0_331( result, joint->node[1].body->posr.R, joint->axis2 );
}
}
dReal dJointGetAMotorParam( dJointID j, int parameter )
{
dxJointAMotor* joint = ( dxJointAMotor* )j;
dAASSERT( joint );
checktype( joint, AMotor );
int anum = parameter >> 8;
if ( anum < 0 ) anum = 0;
if ( anum > 2 ) anum = 2;
parameter &= 0xff;
return joint->limot[anum].get( parameter );
}
void dJointGetPUAxis2( dJointID j, dVector3 result )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
dUASSERT( result, "bad result argument" );
checktype( joint, PU );
if ( joint->flags & dJOINT_REVERSE )
getAxis( joint, result, joint->axis1 );
else
getAxis2( joint, result, joint->axis2 );
}
void dJointSetPUAxis2( dJointID j, dReal x, dReal y, dReal z )
{
dxJointPU* joint = ( dxJointPU* ) j;
dUASSERT( joint, "bad joint argument" );
checktype( joint, PU );
if ( joint->flags & dJOINT_REVERSE )
setAxes( joint, x, y, z, joint->axis1, NULL );
else
setAxes( joint, x, y, z, NULL, joint->axis2 );
joint->computeInitialRelativeRotations();
}
void dJointSetSliderAxis ( dJointID j, dReal x, dReal y, dReal z )
{
dxJointSlider* joint = ( dxJointSlider* ) j;
dUASSERT ( joint, "bad joint argument" );
checktype ( joint, Slider );
setAxes ( joint, x, y, z, joint->axis1, 0 );
joint->computeOffset();
joint->computeInitialRelativeRotation();
}
