static bool EllipseClamp(MT_Scalar& ax, MT_Scalar& az, MT_Scalar *amin, MT_Scalar *amax)
{
MT_Scalar xlim, zlim, x, z;
if (ax < 0.0) {
x = -ax;
xlim = -amin[0];
}
else {
x = ax;
xlim = amax[0];
}
if (az < 0.0) {
z = -az;
zlim = -amin[1];
}
else {
z = az;
zlim = amax[1];
}
if (MT_fuzzyZero(xlim) || MT_fuzzyZero(zlim)) {
if (x <= xlim && z <= zlim)
return false;
if (x > xlim)
x = xlim;
if (z > zlim)
z = zlim;
}
else {
MT_Scalar invx = 1.0/(xlim*xlim);
MT_Scalar invz = 1.0/(zlim*zlim);
if ((x*x*invx + z*z*invz) <= 1.0)
return false;
if (MT_fuzzyZero(x)) {
x = 0.0;
z = zlim;
}
else {
MT_Scalar rico = z/x;
MT_Scalar old_x = x;
x = sqrt(1.0/(invx + invz*rico*rico));
if (old_x < 0.0)
x = -x;
z = rico*x;
}
}
ax = (ax < 0.0)? -x: x;
az = (az < 0.0)? -z: z;
return true;
}
static MT_Vector3 normalize(const MT_Vector3& v)
{
// a sane normalize function that doesn't give (1, 0, 0) in case
// of a zero length vector, like MT_Vector3.normalize
MT_Scalar len = v.length();
return MT_fuzzyZero(len) ? MT_Vector3(0, 0, 0) : v / len;
}
void SCA_ExpressionController::Trigger(SCA_LogicManager* logicmgr)
{
bool expressionresult = false;
if (!m_exprCache)
{
CParser parser;
parser.SetContext(this->AddRef());
m_exprCache = parser.ProcessText(m_exprText);
}
if (m_exprCache)
{
CValue* value = m_exprCache->Calculate();
if (value)
{
if (value->IsError())
{
printf("%s\n", value->GetText().ReadPtr());
} else
{
float num = (float)value->GetNumber();
expressionresult = !MT_fuzzyZero(num);
}
value->Release();
}
}
for (vector<SCA_IActuator*>::const_iterator i=m_linkedactuators.begin();
!(i==m_linkedactuators.end());i++)
{
SCA_IActuator* actua = *i;
logicmgr->AddActiveActuator(actua,expressionresult);
}
}
PyObject *KX_MouseFocusSensor::pyattr_get_ray_direction(void *self_v, const KX_PYATTRIBUTE_DEF *attrdef)
{
KX_MouseFocusSensor* self = static_cast<KX_MouseFocusSensor*>(self_v);
MT_Vector3 dir = self->RayTarget() - self->RaySource();
if (MT_fuzzyZero(dir)) dir.setValue(0,0,0);
else dir.normalize();
return PyObjectFrom(dir);
}
int KX_ConstraintActuator::pyattr_check_direction(void *self, const struct KX_PYATTRIBUTE_DEF *attrdef)
{
KX_ConstraintActuator* act = static_cast<KX_ConstraintActuator*>(self);
MT_Vector3 dir(act->m_refDirection);
MT_Scalar len = dir.length();
if (MT_fuzzyZero(len)) {
PyErr_SetString(PyExc_ValueError, "actuator.direction = vec: KX_ConstraintActuator, invalid direction");
return 1;
}
act->m_refDirVector = dir/len;
return 0;
}
void KX_Camera::NormalizeClipPlanes()
{
if (m_normalized)
return;
for (unsigned int p = 0; p < 6; p++)
{
MT_Scalar factor = sqrt(m_planes[p][0]*m_planes[p][0] + m_planes[p][1]*m_planes[p][1] + m_planes[p][2]*m_planes[p][2]);
if (!MT_fuzzyZero(factor))
m_planes[p] /= factor;
}
m_normalized = true;
}
static MT_Vector3 MatrixToAxisAngle(const MT_Matrix3x3& R)
{
MT_Vector3 delta = MT_Vector3(R[2][1] - R[1][2],
R[0][2] - R[2][0],
R[1][0] - R[0][1]);
MT_Scalar c = safe_acos((R[0][0] + R[1][1] + R[2][2] - 1)/2);
MT_Scalar l = delta.length();
if (!MT_fuzzyZero(l))
delta *= c/l;
return delta;
}
KX_ConstraintActuator::KX_ConstraintActuator(SCA_IObject *gameobj,
int posDampTime,
int rotDampTime,
float minBound,
float maxBound,
float refDir[3],
int locrotxyz,
int time,
int option,
char *property) :
SCA_IActuator(gameobj, KX_ACT_CONSTRAINT),
m_refDirVector(refDir),
m_currentTime(0)
{
m_refDirection[0] = refDir[0];
m_refDirection[1] = refDir[1];
m_refDirection[2] = refDir[2];
m_posDampTime = posDampTime;
m_rotDampTime = rotDampTime;
m_locrot = locrotxyz;
m_option = option;
m_activeTime = time;
if (property) {
m_property = property;
} else {
m_property = "";
}
/* The units of bounds are determined by the type of constraint. To */
/* make the constraint application easier and more transparent later on, */
/* I think converting the bounds to the applicable domain makes more */
/* sense. */
switch (m_locrot) {
case KX_ACT_CONSTRAINT_ORIX:
case KX_ACT_CONSTRAINT_ORIY:
case KX_ACT_CONSTRAINT_ORIZ:
{
MT_Scalar len = m_refDirVector.length();
if (MT_fuzzyZero(len)) {
// missing a valid direction
CM_LogicBrickWarning(this, "there is no valid reference direction!");
m_locrot = KX_ACT_CONSTRAINT_NODEF;
} else {
m_refDirection[0] /= len;
m_refDirection[1] /= len;
m_refDirection[2] /= len;
m_refDirVector /= len;
}
m_minimumBound = cosf(minBound);
m_maximumBound = cosf(maxBound);
m_minimumSine = sinf(minBound);
m_maximumSine = sinf(maxBound);
}
break;
default:
m_minimumBound = minBound;
m_maximumBound = maxBound;
m_minimumSine = 0.f;
m_maximumSine = 0.f;
break;
}
} /* End of constructor */
bool KX_ConstraintActuator::Update(double curtime, bool frame)
{
bool result = false;
bool bNegativeEvent = IsNegativeEvent();
RemoveAllEvents();
if (!bNegativeEvent) {
/* Constraint clamps the values to the specified range, with a sort of */
/* low-pass filtered time response, if the damp time is unequal to 0. */
/* Having to retrieve location/rotation and setting it afterwards may not */
/* be efficient enough... Something to look at later. */
KX_GameObject *obj = (KX_GameObject*) GetParent();
MT_Vector3 position = obj->NodeGetWorldPosition();
MT_Vector3 newposition;
MT_Vector3 normal, direction, refDirection;
MT_Matrix3x3 rotation = obj->NodeGetWorldOrientation();
MT_Scalar filter, newdistance, cosangle;
int axis, sign;
if (m_posDampTime) {
filter = m_posDampTime/(1.0f+m_posDampTime);
} else {
filter = 0.0f;
}
switch (m_locrot) {
case KX_ACT_CONSTRAINT_ORIX:
case KX_ACT_CONSTRAINT_ORIY:
case KX_ACT_CONSTRAINT_ORIZ:
switch (m_locrot) {
case KX_ACT_CONSTRAINT_ORIX:
direction[0] = rotation[0][0];
direction[1] = rotation[1][0];
direction[2] = rotation[2][0];
axis = 0;
break;
case KX_ACT_CONSTRAINT_ORIY:
direction[0] = rotation[0][1];
direction[1] = rotation[1][1];
direction[2] = rotation[2][1];
axis = 1;
break;
default:
direction[0] = rotation[0][2];
direction[1] = rotation[1][2];
direction[2] = rotation[2][2];
axis = 2;
break;
}
if ((m_maximumBound < (1.0f-FLT_EPSILON)) || (m_minimumBound < (1.0f-FLT_EPSILON))) {
// reference direction needs to be evaluated
// 1. get the cosine between current direction and target
cosangle = direction.dot(m_refDirVector);
if (cosangle >= (m_maximumBound-FLT_EPSILON) && cosangle <= (m_minimumBound+FLT_EPSILON)) {
// no change to do
result = true;
goto CHECK_TIME;
}
// 2. define a new reference direction
// compute local axis with reference direction as X and
// Y in direction X refDirection plane
MT_Vector3 zaxis = m_refDirVector.cross(direction);
if (MT_fuzzyZero2(zaxis.length2())) {
// direction and refDirection are identical,
// choose any other direction to define plane
if (direction[0] < 0.9999f)
zaxis = m_refDirVector.cross(MT_Vector3(1.0f,0.0f,0.0f));
else
zaxis = m_refDirVector.cross(MT_Vector3(0.0f,1.0f,0.0f));
}
MT_Vector3 yaxis = zaxis.cross(m_refDirVector);
yaxis.normalize();
if (cosangle > m_minimumBound) {
// angle is too close to reference direction,
// choose a new reference that is exactly at minimum angle
refDirection = m_minimumBound * m_refDirVector + m_minimumSine * yaxis;
} else {
// angle is too large, choose new reference direction at maximum angle
refDirection = m_maximumBound * m_refDirVector + m_maximumSine * yaxis;
}
} else {
refDirection = m_refDirVector;
}
// apply damping on the direction
direction = filter*direction + (1.0f-filter)*refDirection;
obj->AlignAxisToVect(direction, axis);
result = true;
goto CHECK_TIME;
case KX_ACT_CONSTRAINT_DIRPX:
case KX_ACT_CONSTRAINT_DIRPY:
case KX_ACT_CONSTRAINT_DIRPZ:
case KX_ACT_CONSTRAINT_DIRNX:
case KX_ACT_CONSTRAINT_DIRNY:
case KX_ACT_CONSTRAINT_DIRNZ:
switch (m_locrot) {
case KX_ACT_CONSTRAINT_DIRPX:
normal[0] = rotation[0][0];
normal[1] = rotation[1][0];
normal[2] = rotation[2][0];
axis = 0; // axis according to KX_GameObject::AlignAxisToVect()
sign = 0; // X axis will be parrallel to direction of ray
break;
case KX_ACT_CONSTRAINT_DIRPY:
normal[0] = rotation[0][1];
normal[1] = rotation[1][1];
normal[2] = rotation[2][1];
axis = 1;
sign = 0;
break;
case KX_ACT_CONSTRAINT_DIRPZ:
normal[0] = rotation[0][2];
normal[1] = rotation[1][2];
normal[2] = rotation[2][2];
axis = 2;
sign = 0;
break;
case KX_ACT_CONSTRAINT_DIRNX:
normal[0] = -rotation[0][0];
normal[1] = -rotation[1][0];
normal[2] = -rotation[2][0];
axis = 0;
sign = 1;
break;
case KX_ACT_CONSTRAINT_DIRNY:
normal[0] = -rotation[0][1];
normal[1] = -rotation[1][1];
normal[2] = -rotation[2][1];
axis = 1;
sign = 1;
break;
case KX_ACT_CONSTRAINT_DIRNZ:
normal[0] = -rotation[0][2];
normal[1] = -rotation[1][2];
normal[2] = -rotation[2][2];
axis = 2;
sign = 1;
break;
}
normal.normalize();
if (m_option & KX_ACT_CONSTRAINT_LOCAL) {
// direction of the ray is along the local axis
direction = normal;
} else {
switch (m_locrot) {
case KX_ACT_CONSTRAINT_DIRPX:
direction = MT_Vector3(1.0f,0.0f,0.0f);
break;
case KX_ACT_CONSTRAINT_DIRPY:
direction = MT_Vector3(0.0f,1.0f,0.0f);
break;
case KX_ACT_CONSTRAINT_DIRPZ:
direction = MT_Vector3(0.0f,0.0f,1.0f);
break;
case KX_ACT_CONSTRAINT_DIRNX:
direction = MT_Vector3(-1.0f,0.0f,0.0f);
break;
case KX_ACT_CONSTRAINT_DIRNY:
direction = MT_Vector3(0.0f,-1.0f,0.0f);
break;
case KX_ACT_CONSTRAINT_DIRNZ:
direction = MT_Vector3(0.0f,0.0f,-1.0f);
break;
}
}
{
MT_Vector3 topoint = position + (m_maximumBound) * direction;
PHY_IPhysicsEnvironment* pe = KX_GetActiveScene()->GetPhysicsEnvironment();
PHY_IPhysicsController *spc = obj->GetPhysicsController();
if (!pe) {
CM_LogicBrickWarning(this, "there is no physics environment!");
goto CHECK_TIME;
}
if (!spc) {
// the object is not physical, we probably want to avoid hitting its own parent
KX_GameObject *parent = obj->GetParent();
if (parent) {
spc = parent->GetPhysicsController();
}
}
KX_RayCast::Callback<KX_ConstraintActuator, void> callback(this,dynamic_cast<PHY_IPhysicsController*>(spc));
result = KX_RayCast::RayTest(pe, position, topoint, callback);
if (result) {
MT_Vector3 newnormal = callback.m_hitNormal;
// compute new position & orientation
if ((m_option & (KX_ACT_CONSTRAINT_NORMAL|KX_ACT_CONSTRAINT_DISTANCE)) == 0) {
// if none option is set, the actuator does nothing but detect ray
// (works like a sensor)
goto CHECK_TIME;
}
if (m_option & KX_ACT_CONSTRAINT_NORMAL) {
MT_Scalar rotFilter;
// apply damping on the direction
if (m_rotDampTime) {
rotFilter = m_rotDampTime/(1.0f+m_rotDampTime);
} else {
rotFilter = filter;
}
newnormal = rotFilter*normal - (1.0f-rotFilter)*newnormal;
obj->AlignAxisToVect((sign)?-newnormal:newnormal, axis);
if (m_option & KX_ACT_CONSTRAINT_LOCAL) {
direction = newnormal;
direction.normalize();
}
}
if (m_option & KX_ACT_CONSTRAINT_DISTANCE) {
if (m_posDampTime) {
newdistance = filter*(position-callback.m_hitPoint).length()+(1.0f-filter)*m_minimumBound;
} else {
newdistance = m_minimumBound;
}
// logically we should cancel the speed along the ray direction as we set the
// position along that axis
spc = obj->GetPhysicsController();
if (spc && spc->IsDynamic()) {
MT_Vector3 linV = spc->GetLinearVelocity();
// cancel the projection along the ray direction
MT_Scalar fallspeed = linV.dot(direction);
if (!MT_fuzzyZero(fallspeed))
spc->SetLinearVelocity(linV-fallspeed*direction,false);
}
} else {
newdistance = (position-callback.m_hitPoint).length();
}
newposition = callback.m_hitPoint-newdistance*direction;
} else if (m_option & KX_ACT_CONSTRAINT_PERMANENT) {
// no contact but still keep running
result = true;
goto CHECK_TIME;
}
}
break;
case KX_ACT_CONSTRAINT_FHPX:
case KX_ACT_CONSTRAINT_FHPY:
case KX_ACT_CONSTRAINT_FHPZ:
case KX_ACT_CONSTRAINT_FHNX:
case KX_ACT_CONSTRAINT_FHNY:
case KX_ACT_CONSTRAINT_FHNZ:
switch (m_locrot) {
case KX_ACT_CONSTRAINT_FHPX:
normal[0] = -rotation[0][0];
normal[1] = -rotation[1][0];
normal[2] = -rotation[2][0];
direction = MT_Vector3(1.0f,0.0f,0.0f);
break;
case KX_ACT_CONSTRAINT_FHPY:
normal[0] = -rotation[0][1];
normal[1] = -rotation[1][1];
normal[2] = -rotation[2][1];
direction = MT_Vector3(0.0f,1.0f,0.0f);
break;
case KX_ACT_CONSTRAINT_FHPZ:
normal[0] = -rotation[0][2];
normal[1] = -rotation[1][2];
normal[2] = -rotation[2][2];
direction = MT_Vector3(0.0f,0.0f,1.0f);
break;
case KX_ACT_CONSTRAINT_FHNX:
normal[0] = rotation[0][0];
normal[1] = rotation[1][0];
normal[2] = rotation[2][0];
direction = MT_Vector3(-1.0f,0.0f,0.0f);
break;
case KX_ACT_CONSTRAINT_FHNY:
normal[0] = rotation[0][1];
normal[1] = rotation[1][1];
normal[2] = rotation[2][1];
direction = MT_Vector3(0.0f,-1.0f,0.0f);
break;
case KX_ACT_CONSTRAINT_FHNZ:
normal[0] = rotation[0][2];
normal[1] = rotation[1][2];
normal[2] = rotation[2][2];
direction = MT_Vector3(0.0f,0.0f,-1.0f);
break;
}
normal.normalize();
{
PHY_IPhysicsEnvironment* pe = KX_GetActiveScene()->GetPhysicsEnvironment();
PHY_IPhysicsController *spc = obj->GetPhysicsController();
if (!pe) {
CM_LogicBrickWarning(this, "there is no physics environment!");
goto CHECK_TIME;
}
if (!spc || !spc->IsDynamic()) {
// the object is not dynamic, it won't support setting speed
goto CHECK_TIME;
}
m_hitObject = NULL;
// distance of Fh area is stored in m_minimum
MT_Vector3 topoint = position + (m_minimumBound+spc->GetRadius()) * direction;
KX_RayCast::Callback<KX_ConstraintActuator, void> callback(this, spc);
result = KX_RayCast::RayTest(pe, position, topoint, callback);
// we expect a hit object
if (!m_hitObject)
result = false;
if (result)
{
MT_Vector3 newnormal = callback.m_hitNormal;
// compute new position & orientation
MT_Scalar distance = (callback.m_hitPoint-position).length()-spc->GetRadius();
// estimate the velocity of the hit point
MT_Vector3 relativeHitPoint;
relativeHitPoint = (callback.m_hitPoint-m_hitObject->NodeGetWorldPosition());
MT_Vector3 velocityHitPoint = m_hitObject->GetVelocity(relativeHitPoint);
MT_Vector3 relativeVelocity = spc->GetLinearVelocity() - velocityHitPoint;
MT_Scalar relativeVelocityRay = direction.dot(relativeVelocity);
MT_Scalar springExtent = 1.0f - distance/m_minimumBound;
// Fh force is stored in m_maximum
MT_Scalar springForce = springExtent * m_maximumBound;
// damping is stored in m_refDirection [0] = damping, [1] = rot damping
MT_Scalar springDamp = relativeVelocityRay * m_refDirVector[0];
MT_Vector3 newVelocity = spc->GetLinearVelocity()-(springForce+springDamp)*direction;
if (m_option & KX_ACT_CONSTRAINT_NORMAL)
{
newVelocity+=(springForce+springDamp)*(newnormal-newnormal.dot(direction)*direction);
}
spc->SetLinearVelocity(newVelocity, false);
if (m_option & KX_ACT_CONSTRAINT_DOROTFH)
{
MT_Vector3 angSpring = (normal.cross(newnormal))*m_maximumBound;
MT_Vector3 angVelocity = spc->GetAngularVelocity();
// remove component that is parallel to normal
angVelocity -= angVelocity.dot(newnormal)*newnormal;
MT_Vector3 angDamp = angVelocity * ((m_refDirVector[1]>MT_EPSILON)?m_refDirVector[1]:m_refDirVector[0]);
spc->SetAngularVelocity(spc->GetAngularVelocity()+(angSpring-angDamp), false);
}
} else if (m_option & KX_ACT_CONSTRAINT_PERMANENT) {
// no contact but still keep running
result = true;
}
// don't set the position with this constraint
goto CHECK_TIME;
}
break;
case KX_ACT_CONSTRAINT_LOCX:
case KX_ACT_CONSTRAINT_LOCY:
case KX_ACT_CONSTRAINT_LOCZ:
newposition = position = obj->GetSGNode()->GetLocalPosition();
switch (m_locrot) {
case KX_ACT_CONSTRAINT_LOCX:
Clamp(newposition[0], m_minimumBound, m_maximumBound);
break;
case KX_ACT_CONSTRAINT_LOCY:
Clamp(newposition[1], m_minimumBound, m_maximumBound);
break;
case KX_ACT_CONSTRAINT_LOCZ:
Clamp(newposition[2], m_minimumBound, m_maximumBound);
break;
}
result = true;
if (m_posDampTime) {
newposition = filter*position + (1.0f-filter)*newposition;
}
obj->NodeSetLocalPosition(newposition);
goto CHECK_TIME;
}
if (result) {
// set the new position but take into account parent if any
obj->NodeSetWorldPosition(newposition);
}
CHECK_TIME:
if (result && m_activeTime > 0 ) {
if (++m_currentTime >= m_activeTime)
result = false;
}
}
if (!result) {
m_currentTime = 0;
}
return result;
} /* end of KX_ConstraintActuator::Update(double curtime,double deltatime) */
bool IK_QJacobianSolver::Setup(IK_QSegment *root, std::list<IK_QTask *>& tasks)
{
m_segments.clear();
AddSegmentList(root);
// assign each segment a unique id for the jacobian
std::vector<IK_QSegment *>::iterator seg;
int num_dof = 0;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++) {
(*seg)->SetDoFId(num_dof);
num_dof += (*seg)->NumberOfDoF();
}
if (num_dof == 0)
return false;
// compute task id's and assing weights to task
int primary_size = 0, primary = 0;
int secondary_size = 0, secondary = 0;
MT_Scalar primary_weight = 0.0, secondary_weight = 0.0;
std::list<IK_QTask *>::iterator task;
for (task = tasks.begin(); task != tasks.end(); task++) {
IK_QTask *qtask = *task;
if (qtask->Primary()) {
qtask->SetId(primary_size);
primary_size += qtask->Size();
primary_weight += qtask->Weight();
primary++;
}
else {
qtask->SetId(secondary_size);
secondary_size += qtask->Size();
secondary_weight += qtask->Weight();
secondary++;
}
}
if (primary_size == 0 || MT_fuzzyZero(primary_weight))
return false;
m_secondary_enabled = (secondary > 0);
// rescale weights of tasks to sum up to 1
MT_Scalar primary_rescale = 1.0 / primary_weight;
MT_Scalar secondary_rescale;
if (MT_fuzzyZero(secondary_weight))
secondary_rescale = 0.0;
else
secondary_rescale = 1.0 / secondary_weight;
for (task = tasks.begin(); task != tasks.end(); task++) {
IK_QTask *qtask = *task;
if (qtask->Primary())
qtask->SetWeight(qtask->Weight() * primary_rescale);
else
qtask->SetWeight(qtask->Weight() * secondary_rescale);
}
// set matrix sizes
m_jacobian.ArmMatrices(num_dof, primary_size);
if (secondary > 0)
m_jacobian_sub.ArmMatrices(num_dof, secondary_size);
// set dof weights
int i;
for (seg = m_segments.begin(); seg != m_segments.end(); seg++)
for (i = 0; i < (*seg)->NumberOfDoF(); i++)
m_jacobian.SetDoFWeight((*seg)->DoFId() + i, (*seg)->Weight(i));
return true;
}
bool IK_QSwingSegment::UpdateAngle(const IK_QJacobian &jacobian, MT_Vector3& delta, bool *clamp)
{
if (m_locked[0] && m_locked[1])
return false;
MT_Vector3 dq;
dq.x() = jacobian.AngleUpdate(m_DoF_id);
dq.y() = 0.0;
dq.z() = jacobian.AngleUpdate(m_DoF_id+1);
// Directly update the rotation matrix, with Rodrigues' rotation formula,
// to avoid singularities and allow smooth integration.
MT_Scalar theta = dq.length();
if (!MT_fuzzyZero(theta)) {
MT_Vector3 w = dq*(1.0/theta);
MT_Scalar sine = sin(theta);
MT_Scalar cosine = cos(theta);
MT_Scalar cosineInv = 1-cosine;
MT_Scalar xsine = w.x()*sine;
MT_Scalar zsine = w.z()*sine;
MT_Scalar xxcosine = w.x()*w.x()*cosineInv;
MT_Scalar xzcosine = w.x()*w.z()*cosineInv;
MT_Scalar zzcosine = w.z()*w.z()*cosineInv;
MT_Matrix3x3 M(
cosine + xxcosine, -zsine, xzcosine,
zsine, cosine, -xsine,
xzcosine, xsine, cosine + zzcosine);
m_new_basis = m_basis*M;
RemoveTwist(m_new_basis);
}
else
m_new_basis = m_basis;
if (m_limit_x == false && m_limit_z == false)
return false;
MT_Vector3 a = SphericalRangeParameters(m_new_basis);
MT_Scalar ax = 0, az = 0;
clamp[0] = clamp[1] = false;
if (m_limit_x && m_limit_z) {
ax = a.x();
az = a.z();
if (EllipseClamp(ax, az, m_min, m_max))
clamp[0] = clamp[1] = true;
}
else if (m_limit_x) {
if (ax < m_min[0]) {
ax = m_min[0];
clamp[0] = true;
}
else if (ax > m_max[0]) {
ax = m_max[0];
clamp[0] = true;
}
}
else if (m_limit_z) {
if (az < m_min[1]) {
az = m_min[1];
clamp[1] = true;
}
else if (az > m_max[1]) {
az = m_max[1];
clamp[1] = true;
}
}
if (clamp[0] == false && clamp[1] == false)
return false;
m_new_basis = ComputeSwingMatrix(ax, az);
delta = MatrixToAxisAngle(m_basis.transposed()*m_new_basis);
delta[1] = delta[2]; delta[2] = 0.0;
return true;
}
