C++ (Cpp) quat_product Examples

Example #1

File: mathutils.cpp Project: fhedberg/last_letter

void quat_vector3_rotate (geometry_msgs::Quaternion q, geometry_msgs::Vector3 v, geometry_msgs::Vector3 *res)
{
  geometry_msgs::Quaternion vq, q_inv, qa, qb;
  vq.w = 0.0; vq.x = v.x; vq.y = v.y; vq.z = v.z;
  quat_inverse (q, & q_inv);
  quat_product (q, vq, & qa);
  quat_product (qa, q_inv, & qb);
  res -> x = qb.x;
  res -> y = qb.y;
  res -> z = qb.z;
}

Example #2

File: kinematicsLib.cpp Project: Georacer/last_letter

//Propagation of the states
void ForwardEuler::propagation()
{
	geometry_msgs::Vector3 tempVect;

	// Propagate the NED coordinates from earth velocity
	parentObj->states.pose.position.x = parentObj->states.pose.position.x + parentObj->kinematics.posDot.x * parentObj->dt;
	parentObj->states.pose.position.y = parentObj->states.pose.position.y + parentObj->kinematics.posDot.y * parentObj->dt;
	parentObj->states.pose.position.z = parentObj->states.pose.position.z + parentObj->kinematics.posDot.z * parentObj->dt;
	tempVect.x = parentObj->states.pose.position.x;
	tempVect.y = parentObj->states.pose.position.y;
	tempVect.z = parentObj->states.pose.position.z;
	if (isnan(tempVect)) {ROS_FATAL("NaN member in position vector"); ros::shutdown();}

	// Propagate orientation quaternion from angular derivatives quaternion
	geometry_msgs::Quaternion quat = parentObj->states.pose.orientation;
	quat_product(quat,parentObj->kinematics.quatDot,&(parentObj->states.pose.orientation));
	quat_normalize(&(parentObj->states.pose.orientation));
	if (isnan(parentObj->states.pose.orientation)) {ROS_FATAL("NaN member in orientation quaternion"); ros::shutdown();}

	// Propagate body velocity from body velocity derivatives
	tempVect = parentObj->dt*parentObj->kinematics.speedDot;
	parentObj->states.velocity.linear = parentObj->states.velocity.linear + tempVect;
	if (isnan(parentObj->states.velocity.linear)) {ROS_FATAL("NaN member in linear velocity vector"); ros::shutdown();}

	// Propagates angular velocity from angular derivatives
	tempVect = parentObj->dt*parentObj->kinematics.rateDot;
	parentObj->states.velocity.angular = parentObj->states.velocity.angular + tempVect;
	if (isnan(parentObj->states.velocity.angular)) {ROS_FATAL("NaN member in angular velocity vector"); ros::shutdown();}

	// Set linear acceleration from the speed derivatives
	parentObj->states.acceleration.linear = parentObj->kinematics.speedDot;

	// Set angular acceleration from the angular rate derivatives
	parentObj->states.acceleration.angular = parentObj->kinematics.rateDot;

	//Update Geoid stuff using the NED coordinates
	parentObj->states.geoid.latitude += 180.0/M_PI*asin(parentObj->kinematics.posDot.x * parentObj->dt / WGS84_RM(parentObj->states.geoid.latitude));
	parentObj->states.geoid.longitude += 180.0/M_PI*asin(parentObj->kinematics.posDot.y * parentObj->dt / WGS84_RN(parentObj->states.geoid.longitude));
	parentObj->states.geoid.altitude += -parentObj->kinematics.posDot.z * parentObj->dt;
	parentObj->states.geoid.velocity.x = parentObj->kinematics.posDot.x;
	parentObj->states.geoid.velocity.y = parentObj->kinematics.posDot.y;
	parentObj->states.geoid.velocity.z = -parentObj->kinematics.posDot.z; // Upwards velocity
}