bool KX_RayCast::RayTest(PHY_IPhysicsEnvironment* physics_environment, const MT_Point3& _frompoint, const MT_Point3& topoint, KX_RayCast& callback)
{
if(physics_environment==NULL) return false; /* prevents crashing in some cases */
// Loops over all physics objects between frompoint and topoint,
// calling callback.RayHit for each one.
//
// callback.RayHit should return true to stop looking, or false to continue.
//
// returns true if an object was found, false if not.
MT_Point3 frompoint(_frompoint);
const MT_Vector3 todir( (topoint - frompoint).safe_normalized() );
MT_Point3 prevpoint(_frompoint+todir*(-1.f));
PHY_IPhysicsController* hit_controller;
while((hit_controller = physics_environment->rayTest(callback,
frompoint.x(),frompoint.y(),frompoint.z(),
topoint.x(),topoint.y(),topoint.z())) != NULL)
{
KX_ClientObjectInfo* info = static_cast<KX_ClientObjectInfo*>(hit_controller->getNewClientInfo());
if (!info)
{
printf("no info!\n");
MT_assert(info && "Physics controller with no client object info");
break;
}
// The biggest danger to endless loop, prevent this by checking that the
// hit point always progresses along the ray direction..
prevpoint -= callback.m_hitPoint;
if (prevpoint.length2() < MT_EPSILON)
break;
if (callback.RayHit(info))
// caller may decide to stop the loop and still cancel the hit
return callback.m_hitFound;
// Skip past the object and keep tracing.
// Note that retrieving in a single shot multiple hit points would be possible
// but it would require some change in Bullet.
prevpoint = callback.m_hitPoint;
/* We add 0.001 of fudge, so that if the margin && radius == 0., we don't endless loop. */
MT_Scalar marg = 0.001 + hit_controller->GetMargin();
marg *= 2.f;
/* Calculate the other side of this object */
MT_Scalar h = MT_abs(todir.dot(callback.m_hitNormal));
if (h <= 0.01)
// the normal is almost orthogonal to the ray direction, cannot compute the other side
break;
marg /= h;
frompoint = callback.m_hitPoint + marg * todir;
// verify that we are not passed the to point
if ((topoint - frompoint).dot(todir) < 0.f)
break;
}
return false;
}
bool IK_QJacobianSolver::UpdateAngles(MT_Scalar& norm)
{
// assing each segment a unique id for the jacobian
std::vector<IK_QSegment *>::iterator seg;
IK_QSegment *qseg, *minseg = NULL;
MT_Scalar minabsdelta = 1e10, absdelta;
MT_Vector3 delta, mindelta;
bool locked = false, clamp[3];
int i, mindof = 0;
// here we check if any angle limits were violated. angles whose clamped
// position is the same as it was before, are locked immediate. of the
// other violation angles the most violating angle is rememberd
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
qseg = *seg;
if (qseg->UpdateAngle(m_jacobian, delta, clamp)) {
for (i = 0; i < qseg->NumberOfDoF(); i++) {
if (clamp[i] && !qseg->Locked(i)) {
absdelta = MT_abs(delta[i]);
if (absdelta < MT_EPSILON) {
qseg->Lock(i, m_jacobian, delta);
locked = true;
}
else if (absdelta < minabsdelta) {
minabsdelta = absdelta;
mindelta = delta;
minseg = qseg;
mindof = i;
}
}
}
}
}
// lock most violating angle
if (minseg) {
minseg->Lock(mindof, m_jacobian, mindelta);
locked = true;
if (minabsdelta > norm)
norm = minabsdelta;
}
if (locked == false)
// no locking done, last inner iteration, apply the angles
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->UnLock();
(*seg)->UpdateAngleApply();
}
// signal if another inner iteration is needed
return locked;
}
MT_Scalar IK_QJacobian::AngleUpdateNorm() const
{
int i;
MT_Scalar mx = 0.0, dtheta_abs;
for (i = 0; i < m_d_theta.size(); i++) {
dtheta_abs = MT_abs(m_d_theta[i]);
if (dtheta_abs > mx)
mx = dtheta_abs;
}
return mx;
}
float KX_LodManager::GetHysteresis(KX_Scene *scene, unsigned short level)
{
if (!scene->IsActivedLodHysteresis()) {
return 0.0f;
}
KX_LodLevel *lod = m_levels[level];
KX_LodLevel *lodnext = m_levels[level + 1];
float hysteresis = 0.0f;
// if exists, LoD level hysteresis will override scene hysteresis
if (lodnext->GetFlag() & KX_LodLevel::USE_HYSTERESIS) {
hysteresis = lodnext->GetHysteresis() / 100.0f;
}
else {
hysteresis = scene->GetLodHysteresisValue() / 100.0f;
}
return MT_abs(lodnext->GetDistance() - lod->GetDistance()) * hysteresis;
}
static MT_Vector3 SphericalRangeParameters(const MT_Matrix3x3& R)
{
// compute twist parameter
MT_Scalar tau = ComputeTwist(R);
// compute swing parameters
MT_Scalar num = 2.0*(1.0 + R[1][1]);
// singularity at pi
if (MT_abs(num) < MT_EPSILON)
// TODO: this does now rotation of size pi over z axis, but could
// be any axis, how to deal with this i'm not sure, maybe don't
// enforce limits at all then
return MT_Vector3(0.0, tau, 1.0);
num = 1.0/sqrt(num);
MT_Scalar ax = -R[2][1]*num;
MT_Scalar az = R[0][1]*num;
return MT_Vector3(ax, tau, az);
}
void IK_QJacobian::InvertSDLS()
{
// Compute the dampeds least squeares pseudo inverse of J.
//
// Since J is usually not invertible (most of the times it's not even
// square), the psuedo inverse is used. This gives us a least squares
// solution.
//
// This is fine when the J*Jt is of full rank. When J*Jt is near to
// singular the least squares inverse tries to minimize |J(dtheta) - dX)|
// and doesn't try to minimize dTheta. This results in eratic changes in
// angle. The damped least squares minimizes |dtheta| to try and reduce this
// erratic behaviour.
//
// The selectively damped least squares (SDLS) is used here instead of the
// DLS. The SDLS damps individual singular values, instead of using a single
// damping term.
MT_Scalar max_angle_change = MT_PI/4.0;
MT_Scalar epsilon = 1e-10;
int i, j;
m_d_theta = 0;
m_min_damp = 1.0;
for (i = 0; i < m_dof; i++) {
m_norm[i] = 0.0;
for (j = 0; j < m_task_size; j+=3) {
MT_Scalar n = 0.0;
n += m_jacobian[j][i]*m_jacobian[j][i];
n += m_jacobian[j+1][i]*m_jacobian[j+1][i];
n += m_jacobian[j+2][i]*m_jacobian[j+2][i];
m_norm[i] += sqrt(n);
}
}
for (i = 0; i<m_svd_w.size(); i++) {
if (m_svd_w[i] <= epsilon)
continue;
MT_Scalar wInv = 1.0/m_svd_w[i];
MT_Scalar alpha = 0.0;
MT_Scalar N = 0.0;
// compute alpha and N
for (j=0; j<m_svd_u.num_rows(); j+=3) {
alpha += m_svd_u[j][i]*m_beta[j];
alpha += m_svd_u[j+1][i]*m_beta[j+1];
alpha += m_svd_u[j+2][i]*m_beta[j+2];
// note: for 1 end effector, N will always be 1, since U is
// orthogonal, .. so could be optimized
MT_Scalar tmp;
tmp = m_svd_u[j][i]*m_svd_u[j][i];
tmp += m_svd_u[j+1][i]*m_svd_u[j+1][i];
tmp += m_svd_u[j+2][i]*m_svd_u[j+2][i];
N += sqrt(tmp);
}
alpha *= wInv;
// compute M, dTheta and max_dtheta
MT_Scalar M = 0.0;
MT_Scalar max_dtheta = 0.0, abs_dtheta;
for (j = 0; j < m_d_theta.size(); j++) {
MT_Scalar v = m_svd_v[j][i];
M += MT_abs(v)*m_norm[j];
// compute tmporary dTheta's
m_d_theta_tmp[j] = v*alpha;
// find largest absolute dTheta
// multiply with weight to prevent unnecessary damping
abs_dtheta = MT_abs(m_d_theta_tmp[j])*m_weight_sqrt[j];
if (abs_dtheta > max_dtheta)
max_dtheta = abs_dtheta;
}
M *= wInv;
// compute damping term and damp the dTheta's
MT_Scalar gamma = max_angle_change;
if (N < M)
gamma *= N/M;
MT_Scalar damp = (gamma < max_dtheta)? gamma/max_dtheta: 1.0;
for (j = 0; j < m_d_theta.size(); j++) {
// slight hack: we do 0.80*, so that if there is some oscillation,
// the system can still converge (for joint limits). also, it's
// better to go a little to slow than to far
MT_Scalar dofdamp = damp/m_weight[j];
if (dofdamp > 1.0) dofdamp = 1.0;
m_d_theta[j] += 0.80*dofdamp*m_d_theta_tmp[j];
}
if (damp < m_min_damp)
m_min_damp = damp;
}
// weight + prevent from doing angle updates with angles > max_angle_change
MT_Scalar max_angle = 0.0, abs_angle;
for (j = 0; j<m_dof; j++) {
m_d_theta[j] *= m_weight[j];
abs_angle = MT_abs(m_d_theta[j]);
if (abs_angle > max_angle)
max_angle = abs_angle;
}
if (max_angle > max_angle_change) {
MT_Scalar damp = (max_angle_change)/(max_angle_change + max_angle);
for (j = 0; j<m_dof; j++)
m_d_theta[j] *= damp;
}
}