/// \param angles The orientation (unit) vector that you want to reflect
/// \return Euler angles that are reflected about the slope of the plane.
EulerAngles Plane::reflectOrientation(const EulerAngles& angles) const
{
EulerAngles out;
Vector3 look, up, right;
RotationMatrix rotMat;
// get look and up vector from euler angles
rotMat.setup(angles);
look = rotMat.objectToInertial(Vector3(0.0f,0.0f,1.0f));
up = rotMat.objectToInertial(Vector3(0.0f,1.0f,0.0f));
// reflect the look and up vectors over the plane
look = reflectOrientation(look);
up = -reflectOrientation(up);
// calculate right vector
right = Vector3::crossProduct( up, look);
right.normalize();
// create a rotation matrix from right, up, and look
rotMat.m11 = right.x; rotMat.m12 = right.y; rotMat.m13 = right.z;
rotMat.m21 = up.x; rotMat.m22 = up.y; rotMat.m23 = up.z;
rotMat.m31 = look.x; rotMat.m32 = look.y; rotMat.m33 = look.z;
// calculate new euler angles from the matrix
out.fromRotationMatrix(rotMat);
return out;
}
// Resets camera to behind target object
void TetherCamera::reset()
{
Vector3 target;
float targetHeading;
assert(m_objects); // object manager doesn't exist
GameObject* obj = m_objects->getObjectPointer(m_targetObjectID);
assert(obj); // object to follow doesn't exist
targetHeading = obj->getOrientation().heading;
target = obj->getPosition();
target.y += 3.0f;
cameraOrient.set(targetHeading, 0.0f,0.0f);
RotationMatrix cameraMatrix;
cameraMatrix.setup(cameraOrient);
Vector3 bOffset(0.0f,0.0f, -maxDist);
Vector3 iOffset = cameraMatrix.objectToInertial(bOffset);
cameraPos = target + iOffset;
}
RotationMatrix ForwardKinematics::calcChestFeetRotation(const KinematicChain& theKinematicChain)
{
RotationMatrix calculatedRotation;
// calculate rotation based on foot - torso transformation
const Pose3D& footLeft = theKinematicChain.theLinks[KinematicChain::LFoot].M;
const Pose3D& footRight = theKinematicChain.theLinks[KinematicChain::RFoot].M;
const Pose3D& body = theKinematicChain.theLinks[KinematicChain::Torso].M;
// local in chest
Pose3D localFootLeft(body.local(footLeft));
Pose3D localFootRight(body.local(footRight));
if(abs(localFootLeft.translation.z - localFootRight.translation.z) < 3.f/* magic number */)
{
// use average of the calculated rotation of each leg
double meanX = (localFootLeft.rotation.getXAngle() + localFootRight.rotation.getXAngle())*0.5;
double meanY = (localFootLeft.rotation.getYAngle() + localFootRight.rotation.getYAngle())*0.5;
//calculatedRotation.fromKardanRPY(0.0, meanY, meanX);
calculatedRotation.rotateX(meanX).rotateY(meanY);
}
else if(localFootLeft.translation.z > localFootRight.translation.z)
{
// use left foot
calculatedRotation = localFootLeft.rotation;
}
else
{
// use right foot
calculatedRotation = localFootRight.rotation;
}
return calculatedRotation.invert();
}//end calcChestFeetRotation
void Matrix4x3::setupParentToLocal(const Vector3& pos, const EulerAngles& orient)
{
RotationMatrix orientMatrix;
orientMatrix.setup(orient);
setupParentToLocal(pos,orientMatrix);
}
//Rotate by a random amount.
void Coordinates::RotateRand
(XYZArray & dest, uint & pStart, uint & pLen,
const uint m, const uint b, const double max)
{
//Rotate (-max, max) radians about a uniformly random vector
//Not uniformly random, but symmetrical wrt detailed balance
RotationMatrix matrix = RotationMatrix::FromAxisAngle(
prngRef.Sym(max), prngRef.PickOnUnitSphere());
XYZ center = comRef.Get(m);
uint stop = 0;
molRef.GetRange(pStart, stop, pLen, m);
//Copy coordinates
CopyRange(dest, pStart, 0, pLen);
boxDimRef.UnwrapPBC(dest, b, center);
//Do rotation
for (uint p = 0; p < pLen; p++) //Rotate each point.
{
dest.Add(p, -center);
dest.Set(p, matrix.Apply(dest.Get(p)));
dest.Add(p, center);
}
boxDimRef.WrapPBC(dest, b);
}
void KickModule::commandLegsRelativeToTorso(float *command_angles, Pose3D left_target, Pose3D right_target, float /*tilt*/, float roll, float /*tilt_roll_factor*/, bool left_compliant, bool right_compliant) {
RotationMatrix rot;
RotationMatrix foot_rotation;
rot.rotateX(roll);
foot_rotation.rotateX(-roll);
if (left_compliant) {
left_target.translation = rot * left_target.translation;
left_target.rotation = left_target.rotation * foot_rotation;
}
if (right_compliant) {
right_target.translation = rot * right_target.translation;
right_target.rotation = right_target.rotation * foot_rotation;
}
bool isValid = false;
//cout << "*** command legs relative to torso " << endl;
//cout << "commandLegsRel() left foot x y z " << left_target.translation.x << ", " << left_target.translation.y << ", " << left_target.translation.z << endl;
//cout << "left foot rotation x y z " << left_target.rotation.getXAngle() << ", " << left_target.rotation.getYAngle() << ", " << left_target.rotation.getZAngle() << endl;
//cout << "commandLegsRel() right foot x y z " << right_target.translation.x << ", " << right_target.translation.y << ", " << right_target.translation.z << endl;
//cout << "right foot rotation x y z " << right_target.rotation.getXAngle() << ", " << right_target.rotation.getXAngle() << ", " << right_target.rotation.getXAngle() << endl;
isValid = inverse_kinematics_.calcLegJoints(left_target, right_target, command_angles, robot_info_->dimensions_);
command_angles[LHipYawPitch] = 0.0;
command_angles[RHipYawPitch] = 0.0;
}
void BulletObject::updateRay()
{
EulerAngles lookEuler = getOrientation();
lookEuler.bank = 0.0f;
RotationMatrix look;
look.setup(lookEuler);
m_bulletRay = look.objectToInertial(Vector3(0.0f,0.0f,m_range));
}
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 KickEngineData::calcLegJoints(const Joints::Joint& joint, JointRequest& jointRequest, const RobotDimensions& theRobotDimensions, const DamageConfigurationBody& theDamageConfigurationBody)
{
const int sign = joint == Joints::lHipYawPitch ? 1 : -1;
const Vector3f& footPos = (sign > 0) ? positions[Phase::leftFootTra] : positions[Phase::rightFootTra];
const Vector3f& footRotAng = (sign > 0) ? positions[Phase::leftFootRot] : positions[Phase::rightFootRot];
const RotationMatrix rotateBodyTilt = RotationMatrix::aroundX(bodyAngle.x()).rotateY(bodyAngle.y());
Vector3f anklePos = rotateBodyTilt * (footPos - Vector3f(0.f, 0, -theRobotDimensions.footHeight));
anklePos -= Vector3f(0.f, sign * (theRobotDimensions.yHipOffset), 0);
//const RotationMatrix zRot = RotationMatrix::aroundZ(footRotAng.z()).rotateX(sign * pi_4);
//anklePos = zRot * anklePos;
const float leg0 = 0;//std::atan2(-anklePos.x(), anklePos.y());
const float diagonal = anklePos.norm();
// 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(std::abs(a1) > 1.f) OUTPUT_TEXT("clipped a1");
a1 = std::abs(a1) > 1.f ? 0.f : std::acos(a1);
float a2 = (theRobotDimensions.upperLegLength * theRobotDimensions.upperLegLength +
theRobotDimensions.lowerLegLength * theRobotDimensions.lowerLegLength - diagonal * diagonal) /
(2 * theRobotDimensions.upperLegLength * theRobotDimensions.lowerLegLength);
//if(std::abs(a2) > 1.f) OUTPUT_TEXT("clipped a2");
a2 = std::abs(a2) > 1.f ? pi : std::acos(a2);
const float leg2 = -a1 - std::atan2(anklePos.x(), Vector2f(anklePos.y(), anklePos.z()).norm() * -sgn(anklePos.z()));
const float leg1 = anklePos.z() == 0.0f ? 0.0f : (std::atan(anklePos.y() / -anklePos.z()));
const float leg3 = pi - a2;
const RotationMatrix rotateBecauseOfHip = RotationMatrix::aroundZ(0).rotateX(bodyAngle.x()).rotateY(bodyAngle.y());
//calculate inverse foot rotation so that they are flat to the ground
RotationMatrix footRot = RotationMatrix::aroundX(leg1).rotateY(leg2 + leg3);
footRot = footRot.inverse() /* * zRot*/ * rotateBecauseOfHip;
//and add additonal foot rotation (which is probably not flat to the ground)
const float leg4 = std::atan2(footRot(0, 2), footRot(2, 2)) + footRotAng.y();
const float leg5 = std::asin(-footRot(1, 2)) + footRotAng.x() ;
jointRequest.angles[joint] = leg0;
jointRequest.angles[joint + 1] = (/*-pi_4 * sign + */leg1);
jointRequest.angles[joint + 2] = leg2;
jointRequest.angles[joint + 3] = leg3;
jointRequest.angles[joint + 4] = leg4;
jointRequest.angles[joint + 5] = leg5;
//quickhack which allows calibration, but works only if the left foot is the support foot in .kmc file
Vector2f tiltCalibration = currentKickRequest.mirror ? theDamageConfigurationBody.startTiltRight : theDamageConfigurationBody.startTiltLeft;
if(currentKickRequest.mirror)
tiltCalibration.x() *= -1.f;
jointRequest.angles[Joints::lAnkleRoll] += tiltCalibration.x();
jointRequest.angles[Joints::lAnklePitch] += tiltCalibration.y();
}
/// \param pos Specifies the position of the local space within the
/// parent space.
/// \param orient Specifies the orientation of the local space within
/// the parent space as an Euler angle triplet.
void Matrix4x3::setupParentToLocal(const Vector3 &pos, const EulerAngles &orient) {
// Create a rotation matrix.
RotationMatrix orientMatrix;
orientMatrix.setup(orient);
// Setup the 4x3 matrix.
setupParentToLocal(pos, orientMatrix);
}
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;
}
bool KickViewMath::calcLegJoints(const Vector3f& footPos, const Vector3f& 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;
Vector3f anklePos; //FLOAT
anklePos = Vector3f(footPos.x(), footPos.y(), footPos.z() + robotDimensions.footHeight);
anklePos -= Vector3f(0.f, sign * (robotDimensions.yHipOffset), 0.f);
const RotationMatrix rotateBecauseOfHip = RotationMatrix::aroundZ(sign * footRotAng.z()).rotateX(-sign * pi_4);
Vector3f rotatedFootRotAng;
anklePos = rotateBecauseOfHip * anklePos;
leg0 = footRotAng.z();
rotatedFootRotAng = rotateBecauseOfHip * footRotAng;
leg2 = -std::atan2(anklePos.x(), Vector2f(anklePos.y(), anklePos.z()).norm());
float diagonal = anklePos.norm();
// 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);
if(!Rangef::OneRange().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::aroundX(leg1 * -sign).rotateY(leg2 + leg3);
footRot = footRot.inverse() * rotateBecauseOfHip;
leg5 = std::asin(-footRot.col(2).y()) * -sign;
leg4 = -std::atan2(footRot.col(2).x(), footRot.col(2).z()) * -1;
leg4 += footRotAng.y();
leg5 += footRotAng.x();
return legTooShort;
}
/// \param pos Specifies the position of the local space within the
/// parent space.
/// \param orient Specifies the orientation of the local space within
/// the parent space as an Euler angle triplet.
void Matrix4x3::setupLocalToParent(const Vector3 &pos, const EulerAngles &orient) {
// Create a rotation matrix.
RotationMatrix orientMatrix;
orientMatrix.setup(orient);
// Setup the 4x3 matrix. Note: if we were really concerned with
// speed, we could create the matrix directly into these variables,
// without using the temporary RotationMatrix object. This would
// save us a function call and a few copy operations.
setupLocalToParent(pos, orientMatrix);
}
bool InverseKinematics::calcLegJoints(const Pose3D& position, Joints jointAngles, float joint0, bool left, const RobotDimensions& robotDimensions) {
Pose3D target(position);
Joint firstJoint(left ? LHipYawPitch : RHipYawPitch);
const int sign(left ? -1 : 1);
target.translation.y += robotDimensions.values_[RobotDimensions::lengthBetweenLegs] / 2 * sign; // translate to origin of leg
target = Pose3D().rotateZ(joint0 * -sign).rotateX(M_PI_4 * sign).conc(target); // compute residual transformation with fixed joint0
// use cosine theorem and arctan to compute first three joints
float length = target.translation.abs();
float sqrLength = length * length;
float upperLeg = robotDimensions.values_[RobotDimensions::upperLegLength];
float sqrUpperLeg = upperLeg * upperLeg;
float lowerLeg = robotDimensions.values_[RobotDimensions::lowerLegLength];
float sqrLowerLeg = lowerLeg * lowerLeg;
float cosUpperLeg = (sqrUpperLeg + sqrLength - sqrLowerLeg) / (2 * upperLeg * length);
float cosKnee = (sqrUpperLeg + sqrLowerLeg - sqrLength) / (2 * upperLeg * lowerLeg);
// clip for the case that target position is not reachable
const Range<float> clipping(-1.0f, 1.0f);
bool reachable = true;
if(!clipping.isInside(cosKnee) || clipping.isInside(upperLeg))
{
cosKnee = clipping.limit(cosKnee);
cosUpperLeg = clipping.limit(cosUpperLeg);
reachable = false;
}
float joint1 = target.translation.z == 0.0f ? 0.0f : atanf(target.translation.y / -target.translation.z) * sign;
float joint2 = -acos(cosUpperLeg);
joint2 -= atan2(target.translation.x, Vector2<float>(target.translation.y, target.translation.z).abs() * -sgn(target.translation.z));
float joint3 = M_PI - acos(cosKnee);
RotationMatrix beforeFoot = RotationMatrix().rotateX(joint1 * sign).rotateY(joint2 + joint3);
joint1 -= M_PI_4; // because of the strange hip of Nao
// compute joints from rotation matrix using theorem of euler angles
// see http://www.geometrictools.com/Documentation/EulerAngles.pdf
// this is possible because of the known order of joints (y, x, z) where z is left open and is seen as failure
RotationMatrix foot = beforeFoot.invert() * target.rotation;
float joint5 = asin(-foot[2].y) * -sign * -1;
float joint4 = -atan2(foot[2].x, foot[2].z) * -1;
//float failure = atan2(foot[0].y, foot[1].y) * sign;
// set computed joints in jointAngles
jointAngles[firstJoint + 0] = joint0;
jointAngles[firstJoint + 1] = joint1;
jointAngles[firstJoint + 2] = joint2;
jointAngles[firstJoint + 3] = joint3;
jointAngles[firstJoint + 4] = joint4;
jointAngles[firstJoint + 5] = joint5;
return reachable;
}
bool Ned3DObjectManager::interactPlaneTerrain(PlaneObject &plane, TerrainObject &terrain)
{
Terrain *terr = terrain.getTerrain();
if(terr == NULL) return false;
//test for plane collision with terrain
Vector3 planePos = plane.getPosition();
EulerAngles planeOrient = plane.getOrientation();
Vector3 disp = planePos - disp;
RotationMatrix planeMatrix;
planeMatrix.setup(plane.getOrientation()); // get plane's orientation
float planeBottom = plane.getBoundingBox().min.y;
float terrainHeight = terr->getHeight(planePos.x,planePos.z);
if(plane.isPlaneAlive() && planeBottom < terrainHeight)
{ //collision
Vector3 viewVector = planeMatrix.objectToInertial(Vector3(0,0,1));
if(viewVector * terr->getNormal(planePos.x,planePos.z) < -0.5f // dot product
|| plane.isCrashing())
{
plane.killPlane();
int partHndl = gParticle.createSystem("planeexplosion");
gParticle.setSystemPos(partHndl, plane.getPosition());
int boomHndl = gSoundManager.requestSoundHandle("Boom.wav");
int boomInst = gSoundManager.requestInstance(boomHndl);
if(boomInst != SoundManager::NOINSTANCE)
{
gSoundManager.setPosition(boomHndl,boomInst,plane.getPosition());
gSoundManager.play(boomHndl,boomInst);
gSoundManager.releaseInstance(boomHndl,boomInst);
}
plane.setSpeed(0.0f);
planePos += 2.0f * viewVector;
planeOrient.pitch = kPi / 4.0f;
planeOrient.bank = kPi / 4.0f;
plane.setOrientation(planeOrient);
}
else planePos.y = terrainHeight + planePos.y - planeBottom;
//plane.setPPosition(planePos);
return true;
}
return false;
}
void PlaneObject::damage(int hp)
{
// if god mode is on leave
if (!takeDamage)
return;
m_hp -= hp;
setTextureAndSmoke(); // change to a texture with more damange on it
if(m_isPlaneAlive && m_hp <= 0)
{
m_planeState = PS_CRASHING;
m_velocity = Vector3::kForwardVector;
m_eaOrient[0].pitch = degToRad(20);
RotationMatrix r; r.setup(m_eaOrient[0]);
m_velocity = r.objectToInertial(m_velocity);
m_velocity *= m_maxSpeed * m_speedRatio * 20.0f;
}
}
bool Ned3DObjectManager::interactPlaneWater(PlaneObject &plane, WaterObject &water)
{
Water *pWater = water.getWater();
if(pWater == NULL) return false;
// Test for plane collision with water
Vector3 planePos = plane.getPosition();
EulerAngles planeOrient = plane.getOrientation();
Vector3 disp = planePos - disp;
RotationMatrix planeMatrix;
planeMatrix.setup(plane.getOrientation()); // get plane's orientation
float planeBottom = plane.getBoundingBox().min.y;
float waterHeight = pWater->getWaterHeight();
if(plane.isPlaneAlive() && planeBottom < waterHeight)
{ //collision
Vector3 viewVector = planeMatrix.objectToInertial(Vector3(0,0,1));
plane.killPlane();
plane.setSpeed(0.0f);
planePos += 2.0f * viewVector;
planeOrient.pitch = kPi / 4.0f;
planeOrient.bank = kPi / 4.0f;
plane.setOrientation(planeOrient);
plane.setPPosition(planePos);
int partHndl = gParticle.createSystem("planeexplosion");
gParticle.setSystemPos(partHndl, plane.getPosition());
int boomHndl = gSoundManager.requestSoundHandle("Boom.wav");
int boomInst = gSoundManager.requestInstance(boomHndl);
if(boomInst != SoundManager::NOINSTANCE)
{
gSoundManager.setPosition(boomHndl,boomInst,plane.getPosition());
gSoundManager.play(boomHndl,boomInst);
gSoundManager.releaseInstance(boomHndl,boomInst);
}
return true;
}
return false;
}
bool Geometry::calculatePointByAngles
(
const Vector2<>& angles,
const CameraMatrix& cameraMatrix,
const CameraInfo& cameraInfo,
Vector2<int>& point
)
{
Vector3<> vectorToPointWorld, vectorToPoint;
vectorToPointWorld.x = (float) cos(angles.x);
vectorToPointWorld.y = (float) sin(angles.x);
vectorToPointWorld.z = (float) tan(angles.y);
RotationMatrix rotationMatrix = cameraMatrix.rotation;
vectorToPoint = rotationMatrix.invert() * vectorToPointWorld;
float factor = cameraInfo.focalLength;
float scale = factor / vectorToPoint.x;
point.x = (int)(0.5f + cameraInfo.opticalCenter.x - vectorToPoint.y * scale);
point.y = (int)(0.5f + cameraInfo.opticalCenter.y - vectorToPoint.z * scale);
return vectorToPoint.x > 0;
}
void Motion::debugPlots()
{
// some basic plots
// plotting sensor data
PLOT("Motion:GyrometerData:data:x", getGyrometerData().data.x);
PLOT("Motion:GyrometerData:data:y", getGyrometerData().data.y);
PLOT("Motion:GyrometerData:rawData:x", getGyrometerData().rawData.x);
PLOT("Motion:GyrometerData:rawData:y", getGyrometerData().rawData.y);
PLOT("Motion:GyrometerData:ref", getGyrometerData().ref);
PLOT("Motion:AccelerometerData:x", getAccelerometerData().data.x);
PLOT("Motion:AccelerometerData:y", getAccelerometerData().data.y);
PLOT("Motion:AccelerometerData:z", getAccelerometerData().data.z);
PLOT("Motion:InertialSensorData:x", sin(getInertialSensorData().data.x));
PLOT("Motion:InertialSensorData:y", (getInertialSensorData().data.y));
PLOT("Motion:KinematicChain:oriantation:model:x",
getKinematicChainMotor().theLinks[KinematicChain::Hip].R.getXAngle()
);
PLOT("Motion:KinematicChain:oriantation:model:y",
getKinematicChainMotor().theLinks[KinematicChain::Hip].R.getYAngle()
);
DEBUG_REQUEST("Motion:KinematicChain:orientation_test",
RotationMatrix calculatedRotation =
Kinematics::ForwardKinematics::calcChestFeetRotation(getKinematicChainSensor());
// calculate expected acceleration sensor reading
Vector2d inertialExpected(calculatedRotation.getXAngle(), calculatedRotation.getYAngle());
PLOT("Motion:KinematicChain:oriantation:sensor:x", Math::toDegrees(inertialExpected.x) );
PLOT("Motion:KinematicChain:oriantation:sensor:y", Math::toDegrees(inertialExpected.y) );
);
GTEST_TEST(RotationMatrix, getPackedAngleAxisFaulty)
{
Vector3f vec(1, 2, 3);
AngleAxisf aa(pi - 0.0001f, Vector3f::UnitX());
RotationMatrix r = aa;
Vector3f r1 = aa * vec;
Vector3f r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(pi - 0.0001f, Vector3f::UnitY());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(pi - 0.0001f, Vector3f::UnitZ());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi + 0.0001f, Vector3f::UnitX());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi + 0.0001f, Vector3f::UnitY());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi + 0.0001f, Vector3f::UnitZ());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
r = aa = AngleAxisf(-3.14060259f, Vector3f(-0.496929348f, 0.435349584f, 0.750687659f));
AngleAxisf aa3 = Rotation::AngleAxis::unpack(r.getPackedAngleAxis());
r1 = aa * vec;
r2 = Rotation::AngleAxis::unpack(r.getPackedAngleAxisFaulty()) * vec;
EXPECT_FALSE(r1.isApprox(r2));
}
GTEST_TEST(RotationMatrix, getYAngle)
{
float angle = 0_deg;
RotationMatrix r = RotationMatrix::aroundY(angle);
float yAngle = r.getYAngle();
EXPECT_TRUE(Approx::isEqual(yAngle, angle));
angle = 90_deg;
r = RotationMatrix::aroundY(angle);
yAngle = r.getYAngle();
EXPECT_TRUE(Approx::isEqual(yAngle, angle));
angle = 180_deg;
r = RotationMatrix::aroundY(angle);
yAngle = r.getYAngle();
EXPECT_TRUE(Approx::isEqual(Angle::normalize(yAngle), Angle::normalize(angle)));
angle = -180_deg;
r = RotationMatrix::aroundY(angle);
yAngle = r.getYAngle();
EXPECT_TRUE(Approx::isEqual(Angle::normalize(yAngle), Angle::normalize(angle)));
angle = 360_deg;
r = RotationMatrix::aroundY(angle);
yAngle = r.getYAngle();
EXPECT_TRUE(Approx::isEqual(yAngle, Angle::normalize(angle)));
for(int i = 0; i < RUNS; ++i)
{
const float angle = Random::uniform() * pi;
const RotationMatrix r = RotationMatrix::aroundY(angle);
float yAngle = r.getYAngle();
if(!Approx::isEqual(yAngle, angle, 1e-3f))
EXPECT_TRUE(Approx::isEqual(yAngle, angle, 1e-3f))
<< "yAngle:\n"
<< yAngle << "\n"
<< "angle\n"
<< angle << "\n";
}
}
void Docking::setConfiguration(const ScoreConfiguration& scoreConfiguration, Protein &newReceptor, Protein &newLigand) const
{
// rotate receptor
RotationMatrix rotationMatrix;
rotationMatrix.calculateMatrix(receptor_.getOrder(), RotationAngle(0.0, scoreConfiguration.gamma2));
receptor_.rotateTo(newReceptor, rotationMatrix);
rotationMatrix.calculateMatrix(receptor_.getOrder(), RotationAngle(scoreConfiguration.beta2, 0.0));
newReceptor.rotate(rotationMatrix);
// translate
char buffer[100];
// sprintf(buffer, "%s/%d/%.6lf.dat", "data/trans", receptor_.getOrder(), scoreConfiguration.R);
sprintf(buffer, "%s/%d/%.6lf.dat", translationPath_.c_str(), receptor_.getOrder(), scoreConfiguration.R);
TranslationMatrix translationmatrix(buffer);
newReceptor.translate(translationmatrix);
rotationMatrix.calculateMatrix(receptor_.getOrder(), RotationAngle(0.0, scoreConfiguration.gamma1));
ligand_.rotateTo(newLigand, rotationMatrix);
rotationMatrix.calculateMatrix(receptor_.getOrder(), RotationAngle(scoreConfiguration.beta1, 0.0));
newLigand.rotate(rotationMatrix);
rotationMatrix.calculateMatrix(receptor_.getOrder(), RotationAngle(0.0, scoreConfiguration.alpha));
newLigand.rotate(rotationMatrix);
}
// Write RotationMatrix object r values to global matrixData.
void writeMatrixData(RotationMatrix r)
{
for (int i=0; i < 16; i++) matrixData[i] = r.getMatrixData(i);
}
Vector2<double> AngleEstimator::RotationMatrix::operator-(const RotationMatrix& other) const
{
//return ((const RotationMatrix&)(other.invert() * *this)).getAngleAxis();
const Vector3<double>& result(other * ((const RotationMatrix&)(other.invert() * *this)).getAngleAxis());
return Vector2<double>(result.x, result.y);
}
/// Rendering the particles involves looping through every live particle
/// and writing it's relevant data to a vertex buffer, and then rendering
/// that vertex buffer.
void ParticleEffect::render()
{
if(m_nLiveParticleCount == 0) // make sure we have something to render
return;
if(m_sort)
sort();
// save render states before starting
DWORD lighting;
DWORD alphablend;
DWORD zwrite;
DWORD zenable;
DWORD srcblend;
DWORD destblend;
pD3DDevice->GetRenderState(D3DRS_LIGHTING, &lighting);
pD3DDevice->GetRenderState(D3DRS_ALPHABLENDENABLE, &alphablend);
pD3DDevice->GetRenderState(D3DRS_ZWRITEENABLE, &zwrite);
pD3DDevice->GetRenderState(D3DRS_ZENABLE, &zenable);
pD3DDevice->GetRenderState(D3DRS_SRCBLEND, &srcblend);
pD3DDevice->GetRenderState(D3DRS_DESTBLEND, &destblend);
// set up particle engine states
pD3DDevice->SetRenderState(D3DRS_LIGHTING, FALSE);
pD3DDevice->SetRenderState(D3DRS_ALPHABLENDENABLE, TRUE);
if(m_sort)
pD3DDevice->SetRenderState(D3DRS_ZWRITEENABLE, FALSE);
else
pD3DDevice->SetRenderState(D3DRS_ZWRITEENABLE, FALSE);
pD3DDevice->SetRenderState(D3DRS_ZENABLE, TRUE);
pD3DDevice->SetRenderState(D3DRS_SRCBLEND, D3DBLEND_SRCALPHA);
pD3DDevice->SetRenderState(D3DRS_DESTBLEND, D3DBLEND_INVSRCALPHA);
//pD3DDevice->SetRenderState(D3DRS_SRCBLEND, D3DBLEND_SRCALPHA);
//pD3DDevice->SetRenderState(D3DRS_DESTBLEND, D3DBLEND_ZERO);
// set texture operations for alpha blending by setting the color
// to texture color times diffuse color, and alpha taken entirely from
// texture value
pD3DDevice->SetTextureStageState(0,D3DTSS_COLOROP,D3DTOP_MODULATE);
pD3DDevice->SetTextureStageState(0,D3DTSS_COLORARG1,D3DTA_TEXTURE);
pD3DDevice->SetTextureStageState(0,D3DTSS_COLORARG2,D3DTA_DIFFUSE);
pD3DDevice->SetTextureStageState(0,D3DTSS_ALPHAOP,D3DTOP_MODULATE);
pD3DDevice->SetTextureStageState(0,D3DTSS_ALPHAARG1,D3DTA_TEXTURE);
pD3DDevice->SetTextureStageState(0,D3DTSS_ALPHAARG2,D3DTA_DIFFUSE);
// get camera right and up vectors to figure out how to orient the sprites
D3DXMATRIXA16 view;
pD3DDevice->GetTransform(D3DTS_VIEW, &view);
Vector3 vecRight = Vector3(view._11, view._21, view._31);
Vector3 vecUp = Vector3(view._12, view._22, view._32);
Vector3 vecForward = Vector3(view._13, view._23, view._33);
// precalculate corners
Vector3 ul, ur, bl, br;
//ul = -vecRight + vecUp; // upper left
//ur = vecRight + vecUp; // upper right
//bl = -vecRight - vecUp; // bottom left
//br = vecRight - vecUp; // bottom right
pD3DDevice->SetTexture(0, m_txtParticleTexture);
if(!m_vertBuffer->lock())
{
return;
}
// shorthand to the current vertex
RenderVertexL *vert = &((*m_vertBuffer)[0]);
// although these values are the same for all particles (except color.alpha),
// you could implement some randomness, at which point there would be a
// reason to assign to them with every iteration of the loop
unsigned int color;
float size = m_fPISize/2.0f; // half of m_fPISize in each direction
float tTop = 0;
float tBottom = 1.0f;
float tLeft = 0;
float tRight = 1.0f;
// loop through all live particles to assign proper values to the vertices
for (int i=0; i<m_nLiveParticleCount; i++)
{
Vector3 pos = m_Particles[m_drawOrder[i]].position;
color = m_Particles[m_drawOrder[i]].color;
Vector3 myUp, myRight;
Quaternion q;
q.setToRotateAboutAxis(vecForward, m_Particles[m_drawOrder[i]].rotation);
RotationMatrix r;
r.fromObjectToInertialQuaternion(q);
myUp = r.objectToInertial(vecUp);
myRight = r.objectToInertial(vecRight);
ul = -myRight + myUp; // upper left
ur = myRight + myUp; // upper right
bl = -myRight - myUp; // bottom left
br = myRight - myUp; // bottom right
vert->p = pos + ul*size;
vert->argb = color;
vert->u = tLeft;
vert->v = tTop;
vert++;
vert->p = pos + ur*size;
vert->argb = color;
vert->u = tRight;
vert->v = tTop;
vert++;
vert->p = pos + bl*size;
vert->argb = color;
vert->u = tLeft;
vert->v = tBottom;
vert++;
vert->p = pos + br*size;
vert->argb = color;
vert->u = tRight;
vert->v = tBottom;
vert++;
}
m_vertBuffer->unlock();
gRenderer.render(
m_vertBuffer,
m_nLiveParticleCount * 4,
m_indexBuffer,
m_nLiveParticleCount * 2);
// restore render states
pD3DDevice->SetRenderState(D3DRS_LIGHTING, lighting);
pD3DDevice->SetRenderState(D3DRS_ALPHABLENDENABLE, alphablend);
pD3DDevice->SetRenderState(D3DRS_ZWRITEENABLE, zwrite);
pD3DDevice->SetRenderState(D3DRS_ZENABLE, zenable);
pD3DDevice->SetRenderState(D3DRS_SRCBLEND, srcblend);
pD3DDevice->SetRenderState(D3DRS_DESTBLEND, destblend);
}
bool InverseKinematics::calcLegJoints(const Pose3D& position, Joints jointAngles, bool left, const RobotDimensions& robotDimensions) {
Pose3D target(position);
Joint firstJoint(left ? LHipYawPitch : RHipYawPitch);
int sign(left ? -1 : 1);
target.translation.y += (float) robotDimensions.values_[RobotDimensions::lengthBetweenLegs] / 2.f * sign; // translate to origin of leg
// rotate by 45° around origin for Nao
// calculating sqrtf(2) is faster than calculating the resp. rotation matrix with getRotationX()
static const float sqrt2_2 = sqrtf(2.0f) * 0.5f;
RotationMatrix rotationX_pi_4 = RotationMatrix(Vector3<float>(1, 0, 0), Vector3<float>(0, sqrt2_2, sqrt2_2 * sign), Vector3<float>(0, sqrt2_2 * -sign, sqrt2_2));
target.translation = rotationX_pi_4 * target.translation;
target.rotation = rotationX_pi_4 * target.rotation;
target = target.invert(); // invert pose to get position of body relative to foot
// use geometrical solution to compute last three joints
float length = target.translation.abs();
float sqrLength = length * length;
float upperLeg = robotDimensions.values_[RobotDimensions::upperLegLength];
float sqrUpperLeg = upperLeg * upperLeg;
float lowerLeg = robotDimensions.values_[RobotDimensions::lowerLegLength];
float sqrLowerLeg = lowerLeg * lowerLeg;
float cosLowerLeg = (sqrLowerLeg + sqrLength - sqrUpperLeg) / (2 * lowerLeg * length);
float cosKnee = (sqrUpperLeg + sqrLowerLeg - sqrLength) / (2 * upperLeg * lowerLeg);
// clip for the case of unreachable position
const Range<float> clipping(-1.0f, 1.0f);
bool reachable = true;
if(!clipping.isInside(cosKnee) || clipping.isInside(cosLowerLeg))
{
cosKnee = clipping.limit(cosKnee);
cosLowerLeg = clipping.limit(cosLowerLeg);
reachable = false;
}
float joint3 = M_PI - acosf(cosKnee); // implicitly solve discrete ambiguousness (knee always moves forward)
float joint4 = -acosf(cosLowerLeg);
joint4 -= atan2f(target.translation.x, Vector2<float>(target.translation.y, target.translation.z).abs());
float joint5 = atan2f(target.translation.y, target.translation.z) * sign;
// calulate rotation matrix before hip joints
RotationMatrix hipFromFoot;
hipFromFoot.rotateX(joint5 * -sign);
hipFromFoot.rotateY(-joint4 - joint3);
// compute rotation matrix for hip from rotation before hip and desired rotation
RotationMatrix hip = hipFromFoot.invert() * target.rotation;
// compute joints from rotation matrix using theorem of euler angles
// see http://www.geometrictools.com/Documentation/EulerAngles.pdf
// this is possible because of the known order of joints (z, x, y seen from body resp. y, x, z seen from foot)
float joint1 = asinf(-hip[2].y) * -sign;
joint1 -= M_PI_4; // because of the 45°-rotational construction of the Nao legs
float joint2 = -atan2f(hip[2].x, hip[2].z);
float joint0 = atan2f(hip[0].y, hip[1].y) * -sign;
// set computed joints in jointAngles
jointAngles[firstJoint + 0] = joint0;
jointAngles[firstJoint + 1] = joint1;
jointAngles[firstJoint + 2] = joint2;
jointAngles[firstJoint + 3] = joint3;
jointAngles[firstJoint + 4] = joint4;
jointAngles[firstJoint + 5] = joint5;
return reachable;
}
void DCFreeHedron::BuildOld(TrialMol& oldMol, uint molIndex)
{
seed.BuildOld(oldMol, molIndex);
PRNG& prng = data->prng;
const CalculateEnergy& calc = data->calc;
const Forcefield& ff = data->ff;
uint nLJTrials = data->nLJTrialsNth;
double* ljWeights = data->ljWeights;
double* inter = data->inter;
double* real = data->real;
double* self = data->self;
double* corr = data->correction;
//get info about existing geometry
oldMol.SetBasis(hed.Focus(), hed.Prev());
//Calculate OldMol Bond Energy &
//Calculate phi weight for nTrials using actual theta of OldMol
hed.ConstrainedAnglesOld(data->nAngleTrials - 1, oldMol);
const XYZ center = oldMol.AtomPosition(hed.Focus());
XYZArray* positions = data->multiPositions;
double prevPhi[MAX_BONDS];
for (uint i = 0; i < hed.NumBond(); ++i)
{
//get position and shift to origin
positions[i].Set(0, oldMol.AtomPosition(hed.Bonded(i)));
data->axes.UnwrapPBC(positions[i], 0, 1, oldMol.GetBox(), center);
positions[i].Add(0, -center);
}
//add anchor atom
positions[hed.NumBond()].Set(0, oldMol.AtomPosition(hed.Prev()));
data->axes.UnwrapPBC(positions[hed.NumBond()], 0, 1,
oldMol.GetBox(), center);
positions[hed.NumBond()].Add(0, -center);
//counting backward to preserve prototype
for (uint lj = nLJTrials; lj-- > 1;)
{
//convert chosen torsion to 3D positions
RotationMatrix spin =
RotationMatrix::UniformRandom(prng(), prng(), prng());
for (uint b = 0; b < hed.NumBond() + 1; ++b)
{
//find positions
positions[b].Set(lj, spin.Apply(positions[b][0]));
positions[b].Add(lj, center);
}
}
for (uint b = 0; b < hed.NumBond() + 1; ++b)
{
positions[b].Add(0, center);
data->axes.WrapPBC(positions[b], oldMol.GetBox());
}
std::fill_n(inter, nLJTrials, 0.0);
std::fill_n(real, nLJTrials, 0.0);
std::fill_n(self, nLJTrials, 0.0);
std::fill_n(corr, nLJTrials, 0.0);
for (uint b = 0; b < hed.NumBond(); ++b)
{
calc.ParticleInter(inter, real, positions[b], hed.Bonded(b),
molIndex, oldMol.GetBox(), nLJTrials);
if(DoEwald){
data->calc.SwapSelf(self, molIndex, hed.Bonded(b), oldMol.GetBox(), nLJTrials);
data->calc.SwapCorrection(corr, oldMol, positions[b], hed.Bonded(b), oldMol.GetBox(), nLJTrials);
}
}
double stepWeight = 0;
calc.ParticleInter(inter, real, positions[hed.NumBond()], hed.Prev(),
molIndex, oldMol.GetBox(), nLJTrials);
if(DoEwald){
data->calc.SwapSelf(self, molIndex, hed.Prev(), oldMol.GetBox(), nLJTrials);
data->calc.SwapCorrection(corr, oldMol, positions[hed.NumBond()], hed.Prev(), oldMol.GetBox(), nLJTrials);
}
for (uint lj = 0; lj < nLJTrials; ++lj)
{
stepWeight += exp(-ff.beta * inter[lj]);
}
for(uint b = 0; b < hed.NumBond(); ++b)
{
oldMol.ConfirmOldAtom(hed.Bonded(b));
}
oldMol.ConfirmOldAtom(hed.Prev());
oldMol.AddEnergy(Energy(hed.GetEnergy(), 0, inter[0], real[0], 0, self[0], corr[0]));
oldMol.MultWeight(hed.GetWeight());
oldMol.MultWeight(stepWeight);
}
void DCFreeHedron::BuildNew(TrialMol& newMol, uint molIndex)
{
seed.BuildNew(newMol, molIndex);
PRNG& prng = data->prng;
const CalculateEnergy& calc = data->calc;
const Forcefield& ff = data->ff;
uint nLJTrials = data->nLJTrialsNth;
double* ljWeights = data->ljWeights;
double* inter = data->inter;
double* real = data->real;
double* self = data->self;
double* corr = data->correction;
//get info about existing geometry
newMol.ShiftBasis(hed.Focus());
const XYZ center = newMol.AtomPosition(hed.Focus());
XYZArray* positions = data->multiPositions;
for (uint i = 0; i < hed.NumBond(); ++i)
{
positions[i].Set(0, newMol.RawRectCoords(hed.BondLength(i),
hed.Theta(i), hed.Phi(i)));
}
//add anchor atom
positions[hed.NumBond()].Set(0, newMol.RawRectCoords(anchorBond, 0, 0));
//counting backward to preserve prototype
for (uint lj = nLJTrials; lj-- > 0;)
{
//convert chosen torsion to 3D positions
RotationMatrix spin =
RotationMatrix::UniformRandom(prng(), prng(), prng());
for (uint b = 0; b < hed.NumBond() + 1; ++b)
{
//find positions
positions[b].Set(lj, spin.Apply(positions[b][0]));
positions[b].Add(lj, center);
}
}
for (uint b = 0; b < hed.NumBond() + 1; ++b)
{
data->axes.WrapPBC(positions[b], newMol.GetBox());
}
std::fill_n(inter, nLJTrials, 0.0);
std::fill_n(real, nLJTrials, 0.0);
std::fill_n(self, nLJTrials, 0.0);
std::fill_n(corr, nLJTrials, 0.0);
for (uint b = 0; b < hed.NumBond(); ++b)
{
calc.ParticleInter(inter, real, positions[b], hed.Bonded(b),
molIndex, newMol.GetBox(), nLJTrials);
if(DoEwald){
data->calc.SwapSelf(self, molIndex, hed.Bonded(b), newMol.GetBox(), nLJTrials);
data->calc.SwapCorrection(corr, newMol, positions[b], hed.Bonded(b), newMol.GetBox(), nLJTrials);
}
}
calc.ParticleInter(inter, real, positions[hed.NumBond()], hed.Prev(),
molIndex, newMol.GetBox(), nLJTrials);
if(DoEwald){
data->calc.SwapSelf(self, molIndex, hed.Prev(), newMol.GetBox(), nLJTrials);
data->calc.SwapCorrection(corr, newMol, positions[hed.NumBond()], hed.Prev(), newMol.GetBox(), nLJTrials);
}
double stepWeight = 0;
for (uint lj = 0; lj < nLJTrials; ++lj)
{
ljWeights[lj] = exp(-ff.beta * inter[lj]);
stepWeight += ljWeights[lj];
}
uint winner = prng.PickWeighted(ljWeights, nLJTrials, stepWeight);
for(uint b = 0; b < hed.NumBond(); ++b)
{
newMol.AddAtom(hed.Bonded(b), positions[b][winner]);
}
newMol.AddAtom(hed.Prev(), positions[hed.NumBond()][winner]);
newMol.AddEnergy(Energy(hed.GetEnergy(), 0, inter[winner], real[winner], 0, self[winner], corr[winner]));
newMol.MultWeight(hed.GetWeight());
newMol.MultWeight(stepWeight);
}
// TODO: Set the other camera matrix's isValid to false if use current camera
void ODECameraProvider::update(CameraMatrix* theCameraMatrix)
{
if(theCameraConfig->usingTopCam)
{
int supfootLink = 0;
RotationMatrix RHeadW;
RotationMatrix RobotRotationW;
RobotRotationW.rotateX(theNaoStructure->supportFootRotWorld.x).rotateY(theNaoStructure->supportFootRotWorld.y).rotateZ(theNaoStructure->supportFootRotWorld.z);
if(theNaoStructure->supportingMode == NaoConfig::SUPPORT_MODE_LEFT || theNaoStructure->supportingMode == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT)
{
supfootLink = NaoStructure::lFootLink;
}else
{
supfootLink = NaoStructure::rFootLink;
}
//RotationMatrix test = theNaoStructure->uLink[supfootLink].R.invert();
RHeadW = /*RobotRotationW * */(theNaoStructure->uLink[NaoStructure::headEndLink].R * theNaoStructure->uLink[supfootLink].R.invert());
//double rx = RHeadW.getXAngle();
//double ry = RHeadW.getYAngle();
//double rz = RHeadW.getZAngle();
//rx = theNaoStructure->uLink[NaoStructure::headEndLink].R.getXAngle();
//ry = theNaoStructure->uLink[NaoStructure::headEndLink].R.getYAngle();
//rz = theNaoStructure->uLink[NaoStructure::headEndLink].R.getZAngle();
theCameraMatrix->translation = RHeadW * Vector3<double>(CameraTopOffsetX, CameraTopOffsetY, CameraTopOffsetZ)
+RobotRotationW * theNaoStructure->uLink[NaoStructure::headEndLink].p ;
//now camera pos relative to support foot, translate to pos relative to robot coordinate
Vector3<double> offset = (theNaoStructure->uLink[NaoStructure::bodyLink].p -theNaoStructure->uLink[supfootLink].p);
offset.z = 0;
theCameraMatrix->translation -= offset;
theCameraMatrix->translation.x=theCameraMatrix->translation.x*1000;
theCameraMatrix->translation.y=theCameraMatrix->translation.y*1000;
theCameraMatrix->translation.z=theCameraMatrix->translation.z*1000;
theCameraMatrix->rotation = RHeadW.rotateY(CameraTopRotY);
theCameraMatrix->isValid = true;
}
if(!theCameraConfig->usingTopCam)
{
int supfootLink = 0;
RotationMatrix RHeadW;
RotationMatrix RobotRotationW;
RobotRotationW.rotateX(theNaoStructure->supportFootRotWorld.x).rotateY(theNaoStructure->supportFootRotWorld.y).rotateZ(theNaoStructure->supportFootRotWorld.z);
if(theNaoStructure->supportingMode == NaoConfig::SUPPORT_MODE_LEFT || theNaoStructure->supportingMode == NaoConfig::SUPPORT_MODE_DOUBLE_LEFT)
{
supfootLink = NaoStructure::lFootLink;
}else
{
supfootLink = NaoStructure::rFootLink;
}
RHeadW = /*RobotRotationW **/ (theNaoStructure->uLink[NaoStructure::headEndLink].R * theNaoStructure->uLink[supfootLink].R.invert());
theCameraMatrix->translation = RHeadW * Vector3<double>(CameraBottomOffsetX, CameraBottomOffsetY, CameraBottomOffsetZ)
+ RobotRotationW * theNaoStructure->uLink[NaoStructure::headEndLink].p ;
//now camera pos relative to support foot, translate to pos relative to robot coordinate
Vector3<double> offset = (theNaoStructure->uLink[NaoStructure::bodyLink].p -theNaoStructure->uLink[supfootLink].p);
offset.z = 0;
theCameraMatrix->translation -= offset;
theCameraMatrix->translation.x=theCameraMatrix->translation.x*1000;
theCameraMatrix->translation.y=theCameraMatrix->translation.y*1000;
theCameraMatrix->translation.z=theCameraMatrix->translation.z*1000;//视觉的单位为mm
theCameraMatrix->rotation = RHeadW.rotateY(CameraBottomRotY);
theCameraMatrix->isValid = true;
}
}
GTEST_TEST(RotationMatrix, getAngleAxis)
{
Vector3f vec(1, 2, 3);
AngleAxisf aa(pi, Vector3f::UnitX());
RotationMatrix r = aa;
Vector3f r1 = aa * vec;
Vector3f r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(pi - 0.000001f, Vector3f::UnitY());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(pi, Vector3f::UnitZ());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi, Vector3f::UnitX());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi + 0.00001f, Vector3f::UnitY());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(-pi, Vector3f::UnitZ());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
EXPECT_TRUE(r1.isApprox(r2));
r = aa = AngleAxisf(-3.14060259f, Vector3f(-0.496929348f, 0.435349584f, 0.750687659f));
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
if(!r1.isApprox(r2, 1e-4f))
EXPECT_TRUE(r1.isApprox(r2, 1e-4f));
aa = AngleAxisf::Identity();
r = RotationMatrix::Identity() * 0.99999f;
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
if(!r1.isApprox(r2))
EXPECT_TRUE(r1.isApprox(r2));
for(int i = 0; i < RUNS; ++i)
{
vec = Vector3f::Random();
r = aa = AngleAxisf(Random::uniform(-pi, pi), Vector3f::Random().normalized());
r1 = aa * vec;
r2 = r.getAngleAxis() * vec;
if(!r1.isApprox(r2, 1e-2f))
EXPECT_TRUE(r1.isApprox(r2, 1e-2f))
<< "r1:\n"
<< r1 << "\n"
<< "r2\n"
<< r2 << "\n";
}
}
