void InverseKinematics::calcArmJoints(const Vector3<float>& position, const float elbowYaw, Joints jointAngles, bool left, const RobotDimensions& robotDimensions) {
Joint firstJoint(left ? LShoulderPitch : RShoulderPitch);
const int sign(left ? -1 : 1);
const Vector3<float> pos(position - Vector3<float>(robotDimensions.values_[RobotDimensions::armOffset1], robotDimensions.values_[RobotDimensions::armOffset2] * -sign, robotDimensions.values_[RobotDimensions::armOffset3]));
float& joint0 = jointAngles[firstJoint + 0];
float& joint1 = jointAngles[firstJoint + 1];
const float& joint2 = jointAngles[firstJoint + 2] = elbowYaw;
float& joint3 = jointAngles[firstJoint + 3];
// distance of the end effector position to the origin
const float positionAbs = pos.abs();
// the upper and lower arm form a triangle with the air line to the end effector position being the third edge. Elbow angle can be computed using cosine theorem
const float actualUpperArmLength = Vector2<float>(robotDimensions.values_[RobotDimensions::upperArmLength], robotDimensions.values_[RobotDimensions::elbowOffsetY]).abs();
float cosElbow = (sqr(actualUpperArmLength) + sqr(robotDimensions.values_[RobotDimensions::lowerArmLength]) - sqr(positionAbs)) / (2.0f * robotDimensions.values_[RobotDimensions::upperArmLength] * robotDimensions.values_[RobotDimensions::lowerArmLength]);
// clip for the case of unreachable position
cosElbow = Range<float>(-1.0f, 1.0f).limit(cosElbow);
// elbow is streched in zero-position, hence M_PI - innerAngle
joint3 = -(M_PI - acos(cosElbow));
// compute temporary end effector position from known third and fourth joint angle
const Pose3D tempPose = Pose3D(robotDimensions.values_[RobotDimensions::upperArmLength], robotDimensions.values_[RobotDimensions::elbowOffsetY] * -sign, 0).rotateX(joint2 * -sign).rotateZ(joint3 * -sign).translate(robotDimensions.values_[RobotDimensions::lowerArmLength], 0, 0);
/* offset caused by third and fourth joint angle */ /* angle needed to realise y-component of target position */
joint1 = atan2(tempPose.translation.y * sign, tempPose.translation.x) + asin(pos.y / Vector2<float>(tempPose.translation.x, tempPose.translation.y).abs()) * -sign;
// refresh temporary endeffector position with known joint1
const Pose3D tempPose2 = Pose3D().rotateZ(joint1 * -sign).conc(tempPose);
/* first compensate offset from temporary position */ /* angle from target x- and z-component of target position */
joint0 = -atan2(tempPose2.translation.z, tempPose2.translation.x) + atan2(pos.z, pos.x);
}
RotationMatrix::RotationMatrix(const Vector3<>& a, float angle)
{
Vector3<> axis = a;
const float axisLen = axis.abs();
if(axisLen != 0.f)
axis *= 1.f / axisLen; // normalize a, rotation is only possible with unit vectors
const float& x = axis.x, &y = axis.y, &z = axis.z;
//compute sine and cosine of angle because it is needed quite often for complete matrix
const float si = sin(angle), co = cos(angle);
//compute all components needed more than once for complete matrix
const float v = 1 - co;
const float xyv = x * y * v;
const float xzv = x * z * v;
const float yzv = y * z * v;
const float xs = x * si;
const float ys = y * si;
const float zs = z * si;
//compute matrix
c0.x = x * x * v + co;
c1.x = xyv - zs;
c2.x = xzv + ys;
c0.y = xyv + zs;
c1.y = y * y * v + co;
c2.y = yzv - xs;
c0.z = xzv - ys;
c1.z = yzv + xs;
c2.z = z * z * v + co;
}
void BoxShapeSW::_setup(const Vector3& p_half_extents) {
half_extents=p_half_extents.abs();
configure(AABB(-half_extents,half_extents*2));
}
void KickEngineData::calcLegJoints(const JointData::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions)
{
float sign = joint == JointData::LHipYawPitch ? 1.f : -1.f;
float leg0, leg1, leg2, leg3, leg4, leg5;
const Vector3<>& footPos = (sign > 0) ? positions[Phase::leftFootTra] : positions[Phase::rightFootTra];
const Vector3<>& footRotAng = (sign > 0) ? positions[Phase::leftFootRot] : positions[Phase::rightFootRot];
RotationMatrix rotateBodyTilt = RotationMatrix().rotateX(comOffset.x);
Vector3<> anklePos = rotateBodyTilt * Vector3<>(footPos.x, footPos.y, footPos.z);
//we need just the leg length x and y have to stay the same
anklePos.y = footPos.y;
anklePos.x = footPos.x;
// for the translation of the footpos we only need to translate the anklepoint, which is the intersection of the axis leg4 and leg5
// the rotation of the foot will be made later by rotating the footpos around the anklepoint
anklePos -= Vector3<>(0.f, sign * (theRobotDimensions.lengthBetweenLegs / 2), -theRobotDimensions.heightLeg5Joint);
RotationMatrix rotateBecauseOfHip = RotationMatrix().rotateZ(footRotAng.z).rotateX(-sign * pi_4);
anklePos = rotateBecauseOfHip * anklePos;
float diagonal = anklePos.abs();
// upperLegLength, lowerLegLength, and diagonal form a triangle, use cosine theorem
float a1 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength -
theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength + diagonal * diagonal) /
(2 * theRobotDimensions.upperLegLength * diagonal);
//if(abs(a1) > 1.f) OUTPUT(idText, text, "clipped a1");
a1 = abs(a1) > 1.f ? 0.f : acos(a1);
float a2 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength +
theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength - diagonal * diagonal) /
(2 * theRobotDimensions.upperLegLength * theRobotDimensions.lowerLegLength);
//if(abs(a2) > 1.f) OUTPUT(idText, text, "clipped a2");
a2 = abs(a2) > 1.f ? pi : acos(a2);
leg0 = footRotAng.z * sign;
leg2 = -a1 - atan2(anklePos.x, Vector2<>(anklePos.y, anklePos.z).abs() * -sgn(anklePos.z));
leg1 = anklePos.z == 0.0f ? 0.0f : atan(anklePos.y / -anklePos.z) * -sign;
leg3 = pi - a2;
RotationMatrix footRot = RotationMatrix().rotateX(leg1 * -sign).rotateY(leg2 + leg3);
footRot = footRot.invert() * rotateBecauseOfHip;
leg5 = asin(-footRot[2].y) * sign * -1;
leg4 = -atan2(footRot[2].x, footRot[2].z) * -1;
leg4 += footRotAng.y;
leg5 += footRotAng.x;
jointRequest.angles[joint] = leg0;
jointRequest.angles[joint + 1] = -pi_4 + leg1;
jointRequest.angles[joint + 2] = leg2;
jointRequest.angles[joint + 3] = leg3;
jointRequest.angles[joint + 4] = leg4;
jointRequest.angles[joint + 5] = leg5;
}
void SimObjectRenderer::rotateCamera(float x, float y)
{
if(cameraMode == SimRobotCore2::Renderer::targetCam)
{
Vector3<> v = cameraPos - cameraTarget;
Matrix3x3<> rotateY(Vector3<>(0.f, y, 0.f));
Matrix3x3<> rotateZ(Vector3<>(0.f, 0.f, x));
Vector3<> v2(sqrtf(v.x * v.x + v.y * v.y), 0.f, v.z);
v2 = rotateY * v2;
if(v2.x < 0.001f)
{
v2.x = 0.001f;
v2.normalize(v.abs());
}
Vector3<> v3(v.x, v.y, 0.f);
v3.normalize(v2.x);
v3.z = v2.z;
v = rotateZ * v3;
cameraPos = cameraTarget + v;
}
else // if(cameraMode == SimRobotCore2::Renderer::freeCam)
{
Vector3<> v = cameraTarget - cameraPos;
Matrix3x3<> rotateY(Vector3<>(0.f, y, 0.f));
Matrix3x3<> rotateZ(Vector3<>(0.f, 0.f, -x));
Vector3<> v2(sqrtf(v.x * v.x + v.y * v.y), 0.f, v.z);
v2 = rotateY * v2;
if(v2.x < 0.001f)
{
v2.x = 0.001f;
v2.normalize(v.abs());
}
Vector3<> v3(v.x, v.y, 0.f);
v3.normalize(v2.x);
v3.z = v2.z;
v = rotateZ * v3;
cameraTarget = cameraPos + v;
}
updateCameraTransformation();
}
bool ViewBikeMath::calcLegJoints(const Vector3<>& footPos, const Vector3<>& footRotAng, const bool& left, const RobotDimensions& robotDimensions, float& leg0, float& leg1, float& leg2, float& leg3, float& leg4, float& leg5)
{
float sign = (left) ? 1.f : -1.f;
bool legTooShort = false;
Vector3<> anklePos; //FLOAT
anklePos = Vector3<>(footPos.x, footPos.y, footPos.z + robotDimensions.heightLeg5Joint);
anklePos -= Vector3<>(0.f, sign * (robotDimensions.lengthBetweenLegs / 2.f), 0.f);
RotationMatrix rotateBecauseOfHip = RotationMatrix().rotateZ(sign * footRotAng.z).rotateX(-sign * pi_4);
Vector3<> rotatedFootRotAng;
anklePos = rotateBecauseOfHip * anklePos;
leg0 = footRotAng.z;
rotatedFootRotAng = rotateBecauseOfHip * footRotAng;
leg2 = -std::atan2(anklePos.x, Vector2<>(anklePos.y, anklePos.z).abs());
float diagonal = anklePos.abs();
// upperLegLength, lowerLegLength, and diagonal form a triangle, use cosine theorem
float a1 = (robotDimensions.upperLegLength * robotDimensions.upperLegLength -
robotDimensions.lowerLegLength * robotDimensions.lowerLegLength + diagonal * diagonal) /
(2 * robotDimensions.upperLegLength * diagonal);
a1 = std::abs(a1) > 1.f ? 0 : std::acos(a1);
float a2 = (robotDimensions.upperLegLength * robotDimensions.upperLegLength +
robotDimensions.lowerLegLength * robotDimensions.lowerLegLength - diagonal * diagonal) /
(2 * robotDimensions.upperLegLength * robotDimensions.lowerLegLength);
const Range<float> clipping(-1.f, 1.f);
if(!clipping.isInside(a2))
{
legTooShort = true;
}
a2 = std::abs(a2) > 1.f ? pi : std::acos(a2);
leg1 = (-sign * std::atan2(anklePos.y, std::abs(anklePos.z))) - (pi / 4);
leg2 -= a1;
leg3 = pi - a2;
RotationMatrix footRot = RotationMatrix().rotateX(leg1 * -sign).rotateY(leg2 + leg3);
footRot = footRot.invert() * rotateBecauseOfHip;
leg5 = std::asin(-footRot[2].y) * -sign;
leg4 = -std::atan2(footRot[2].x, footRot[2].z) * -1;
leg4 += footRotAng.y;
leg5 += footRotAng.x;
return legTooShort;
}
void CameraObject::focus_at (const Vector3 &to, const Vector3 &up, const DTfloat radius)
{
look_at (to, up);
Vector3 diff;
DTfloat dist;
diff = to - translation();
dist = diff.abs();
_angle = RAD_TO_DEG * 2.0F * std::atan(radius / dist);
// Keep from getting too small
if (_angle < 0.1F) _angle = 0.1F;
}
// Decomposes a Basis into a rotation-reflection matrix (an element of the group O(3)) and a positive scaling matrix as B = O.S.
// Returns the rotation-reflection matrix via reference argument, and scaling information is returned as a Vector3.
// This (internal) function is too specific and named too ugly to expose to users, and probably there's no need to do so.
Vector3 Basis::rotref_posscale_decomposition(Basis &rotref) const {
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(determinant() == 0, Vector3());
Basis m = transposed() * (*this);
ERR_FAIL_COND_V(!m.is_diagonal(), Vector3());
#endif
Vector3 scale = get_scale();
Basis inv_scale = Basis().scaled(scale.inverse()); // this will also absorb the sign of scale
rotref = (*this) * inv_scale;
#ifdef MATH_CHECKS
ERR_FAIL_COND_V(!rotref.is_orthogonal(), Vector3());
#endif
return scale.abs();
}
AngleEstimator::RotationMatrix::RotationMatrix(const Vector3<>& angleAxis)
{
const double angle = angleAxis.abs();
const Vector3<> axis(angleAxis / angle);
//rotation is only possible with unit vectors
const double &x = axis.x, &y = axis.y, &z = axis.z;
//compute sine and cosine of angle because it is needed quite often for complete matrix
const double si = sin(angle), co = cos(angle);
//compute all components needed more than once for complete matrix
const double v = 1 - co;
const double xyv = x * y * v;
const double xzv = x * z * v;
const double yzv = y * z * v;
const double xs = x * si;
const double ys = y * si;
const double zs = z * si;
c[0].x = x * x * v + co; c[1].x = xyv - zs; c[2].x = xzv + ys;
c[0].y = xyv + zs; c[1].y = y * y * v + co; c[2].y = yzv - xs;
c[0].z = xzv - ys; c[1].z = yzv + xs; c[2].z = z * z * v + co;
}
template <class V> Matrix3x3<V>::Matrix3x3(const Vector3<V>& a)
{
const V angle = a.abs();
Vector3<V> axis = a;
if(angle != V())
axis /= angle; // normalize a, rotation is only possible with unit vectors
const V &x = axis.x, &y = axis.y, &z = axis.z;
//compute sine and cosine of angle because it is needed quite often for complete matrix
const V si = sin(angle), co = cos(angle);
//compute all components needed more than once for complete matrix
const V v = 1 - co;
const V xyv = x * y * v;
const V xzv = x * z * v;
const V yzv = y * z * v;
const V xs = x * si;
const V ys = y * si;
const V zs = z * si;
//compute matrix
c0.x = x * x * v + co; c1.x = xyv - zs; c2.x = xzv + ys;
c0.y = xyv + zs; c1.y = y * y * v + co; c2.y = yzv - xs;
c0.z = xzv - ys; c1.z = yzv + xs; c2.z = z * z * v + co;
}
void InverseKinematics::calcJointsForElbowPos(const Vector3<float> &elbow, const Vector3<float> &target, Joints jointAngles, int offset, const RobotDimensions &theRobotDimensions)
{
//set elbow position with the pitch/yaw unit in the shoulder
jointAngles[offset + 0] = atan2f(elbow.z, elbow.x);
jointAngles[offset + 1] = atan2f(elbow.y, sqrtf(sqr(elbow.x)+sqr(elbow.z)));
//calculate desired elbow "klapp"-angle
const float c = target.abs(),
a = theRobotDimensions.upperArmLength,
b = theRobotDimensions.lowerArmLength;
//cosine theorem
float cosAngle = (-sqr(c) + sqr(b) + sqr(a))/(2.0f*a*b);
if(cosAngle < -1.0f)
{
assert(cosAngle > -1.1);
cosAngle = -1.0f;
}
else if(cosAngle > 1.0f)
{
assert(cosAngle < 1.1);
cosAngle = 1.0f;
}
jointAngles[offset + 3] = acosf(cosAngle)-M_PI;
//calculate hand in elbow coordinate system and calculate last angle
Pose3D shoulder2Elbow;
shoulder2Elbow.translate(0, -theRobotDimensions.upperArmLength, 0);
shoulder2Elbow.rotateX(-(jointAngles[offset + 1] - M_PI/2.0f));
shoulder2Elbow.rotateY(jointAngles[offset + 0] + M_PI/2.0f);
const Vector3<float> handInEllbow = shoulder2Elbow * target;
jointAngles[offset + 2] = -(atan2f(handInEllbow.z, handInEllbow.x)+M_PI/2);
while(jointAngles[offset + 2] > M_PI)
jointAngles[offset + 2] -= 2*M_PI;
while(jointAngles[offset + 2] < -M_PI)
jointAngles[offset + 2] += 2*M_PI;
}
void SimObjectRenderer::moveCamera(float x, float y)
{
if(cameraMode == SimRobotCore2::Renderer::targetCam)
{
if(x != 0.f)
rotateCamera(x, 0);
if(y != 0.f)
{
Vector3<> v = cameraPos - cameraTarget;
float len = v.abs() + y;
if(len < 0.0001f)
len = 0.0001f;
v.normalize(len);
cameraPos = cameraTarget + v;
}
}
else // if(cameraMode == SimRobotCore2::Renderer::FREECAM)
{
if(x != 0.f)
{
Vector3<> v = cameraTarget - cameraPos;
Vector3<> v2 = v ^ Vector3<>(0.f, 0.f, 1.f);
v2.normalize(x);
cameraTarget += v2;
cameraPos += v2;
}
if(y != 0.f)
{
Vector3<> v = cameraTarget - cameraPos;
v.normalize(y);
cameraTarget -= v;
cameraPos -= v;
}
}
updateCameraTransformation();
}
void SimObjectRenderer::zoom(float change)
{
Vector3<> v = cameraPos - cameraTarget;
moveCamera(0.f, v.abs() * change * 0.0005f);
}
void Simulation::staticCollisionCallback(Simulation* simulation, dGeomID geomId1, dGeomID geomId2)
{
ASSERT(!dGeomIsSpace(geomId1));
ASSERT(!dGeomIsSpace(geomId2));
#ifndef NDEBUG
{
dBodyID bodyId1 = dGeomGetBody(geomId1);
dBodyID bodyId2 = dGeomGetBody(geomId2);
ASSERT(bodyId1 || bodyId2);
Body* body1 = bodyId1 ? (Body*)dBodyGetData(bodyId1) : 0;
Body* body2 = bodyId2 ? (Body*)dBodyGetData(bodyId2) : 0;
ASSERT(!body1 || !body2 || body1->rootBody != body2->rootBody);
}
#endif
dContact contact[32];
int collisions = dCollide(geomId1, geomId2, 32, &contact[0].geom, sizeof(dContact));
if(collisions <= 0)
return;
Geometry* geometry1 = (Geometry*)dGeomGetData(geomId1);
Geometry* geometry2 = (Geometry*)dGeomGetData(geomId2);
if(geometry1->collisionCallbacks && !geometry2->immaterial)
{
for(std::list<SimRobotCore2::CollisionCallback*>::iterator i = geometry1->collisionCallbacks->begin(), end = geometry1->collisionCallbacks->end(); i != end; ++i)
(*i)->collided(*geometry1, *geometry2);
if(geometry1->immaterial)
return;
}
if(geometry2->collisionCallbacks && !geometry1->immaterial)
{
for(std::list<SimRobotCore2::CollisionCallback*>::iterator i = geometry2->collisionCallbacks->begin(), end = geometry2->collisionCallbacks->end(); i != end; ++i)
(*i)->collided(*geometry2, *geometry1);
if(geometry2->immaterial)
return;
}
dBodyID bodyId1 = dGeomGetBody(geomId1);
dBodyID bodyId2 = dGeomGetBody(geomId2);
ASSERT(bodyId1 || bodyId2);
float friction = 1.f;
if(geometry1->material && geometry2->material)
{
if(!geometry1->material->getFriction(*geometry2->material, friction))
friction = 1.f;
float rollingFriction;
if(bodyId1)
switch(dGeomGetClass(geomId1))
{
case dSphereClass:
case dCCylinderClass:
case dCylinderClass:
if(geometry1->material->getRollingFriction(*geometry2->material, rollingFriction))
{
dBodySetAngularDamping(bodyId1, 0.2);
Vector3<> linearVel;
ODETools::convertVector(dBodyGetLinearVel(bodyId1), linearVel);
linearVel -= Vector3<>(linearVel).normalize(std::min(linearVel.abs(), rollingFriction * simulation->scene->stepLength));
dBodySetLinearVel(bodyId1, linearVel.x, linearVel.y, linearVel.z);
}
break;
}
if(bodyId2)
switch(dGeomGetClass(geomId2))
{
case dSphereClass:
case dCCylinderClass:
case dCylinderClass:
if(geometry2->material->getRollingFriction(*geometry1->material, rollingFriction))
{
dBodySetAngularDamping(bodyId2, 0.2);
Vector3<> linearVel;
ODETools::convertVector(dBodyGetLinearVel(bodyId2), linearVel);
linearVel -= Vector3<>(linearVel).normalize(std::min(linearVel.abs(), rollingFriction * simulation->scene->stepLength));
dBodySetLinearVel(bodyId2, linearVel.x, linearVel.y, linearVel.z);
}
break;
}
}
for(dContact* cont = contact, * end = contact + collisions; cont < end; ++cont)
{
cont->surface.mode = simulation->scene->contactMode | dContactApprox1;
cont->surface.mu = friction;
/*
cont->surface.bounce = 0.f;
cont->surface.bounce_vel = 0.001f;
cont->surface.slip1 = 0.f;
cont->surface.slip2 = 0.f;
*/
cont->surface.soft_erp = simulation->scene->contactSoftERP;
cont->surface.soft_cfm = simulation->scene->contactSoftCFM;
dJointID c = dJointCreateContact(simulation->physicalWorld, simulation->contactGroup, cont);
ASSERT(bodyId1 == dGeomGetBody(cont->geom.g1));
ASSERT(bodyId2 == dGeomGetBody(cont->geom.g2));
dJointAttach(c, bodyId1, bodyId2);
}
++simulation->collisions;
simulation->contactPoints += collisions;
}