text: // This file is a part of the OpenSurgSim project.
  // Copyright 2013, SimQuest Solutions Inc.
  //
  // Licensed under the Apache License, Version 2.0 (the "License");
  // you may not use this file except in compliance with the License.
  // You may obtain a copy of the License at
  //
  //     http://www.apache.org/licenses/LICENSE-2.0
  //
  // Unless required by applicable law or agreed to in writing, software
  // distributed under the License is distributed on an "AS IS" BASIS,
  // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  // See the License for the specific language governing permissions and
  // limitations under the License.
  
  #include <boost/thread/locks.hpp>
  
  #include "SurgSim/Devices/DeviceFilters/PoseTransform.h"
  
  #include "SurgSim/Math/MathConvert.h"
  #include "SurgSim/Math/Matrix.h"
  
  using SurgSim::DataStructures::DataGroup;
  using SurgSim::Math::RigidTransform3d;
  using SurgSim::Math::Vector3d;
  
  namespace SurgSim
  {
  namespace Devices
  {
  
  SURGSIM_REGISTER(SurgSim::Input::DeviceInterface, SurgSim::Devices::PoseTransform, PoseTransform);
  
  PoseTransform::PoseTransform(const std::string& name) :
  	DeviceFilter(name),
  	m_transform(RigidTransform3d::Identity()),
  	m_transformInverse(RigidTransform3d::Identity()),
  	m_translationScale(1.0)
  {
  	SURGSIM_ADD_SERIALIZABLE_PROPERTY(PoseTransform, double, TranslationScale,
  		getTranslationScale, setTranslationScale);
  	SURGSIM_ADD_SERIALIZABLE_PROPERTY(PoseTransform, RigidTransform3d, Transform, getTransform, setTransform);
  }
  
  void PoseTransform::filterInput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
  {
  	boost::lock_guard<boost::mutex> lock(m_mutex); // Prevent the transform or scaling from being set simultaneously.
  
  	*result = dataToFilter;  // Pass on all the data entries.
  
  	RigidTransform3d pose; // If there is a pose, scale the translation, then transform the result.
  	if (dataToFilter.poses().get(DataStructures::Names::POSE, &pose))
  	{
  		pose.translation() *= m_translationScale;
  		pose = m_transform * pose;
  		result->poses().set(DataStructures::Names::POSE, pose);
  	}
  
  	// If there is a linear velocity, scale then rotate it.  The linear velocity is scaled because it is the change
  	// in translation over time, and the translation is being scaled.
  	Vector3d linearVelocity;
  	if (dataToFilter.vectors().get(DataStructures::Names::LINEAR_VELOCITY, &linearVelocity))
  	{
  		linearVelocity *= m_translationScale;
  		linearVelocity = m_transform.linear() * linearVelocity;
  		result->vectors().set(DataStructures::Names::LINEAR_VELOCITY, linearVelocity);
  	}
  
  	Vector3d angularVelocity; // If there is an angular velocity, rotate it.
  	if (dataToFilter.vectors().get(DataStructures::Names::ANGULAR_VELOCITY, &angularVelocity))
  	{
  		angularVelocity = m_transform.linear() * angularVelocity;
  		result->vectors().set(DataStructures::Names::ANGULAR_VELOCITY, angularVelocity);
  	}
  }
  
  void PoseTransform::filterOutput(const std::string& device, const DataGroup& dataToFilter, DataGroup* result)
  {
  	boost::lock_guard<boost::mutex> lock(m_mutex); // Prevent the transform or scaling from being set simultaneously.
  
  	*result = dataToFilter;  // Pass on all the data entries.
  
  	// Since the haptic devices will compare the data in the output DataGroup to a raw input pose, the filter must
  	// perform the reverse transform and scaling to data used by the haptic devices.
  
  	// The force and torque must be transformed into device space.  In order to reliably display the desired
  	// forces as calculated by the simulation, the nominal forces and torques are not scaled by the translation scaling.
  	// The benefit is that increasing the translation scaling does not result in larger penetrations for the
  	// same force, but the downside is that as the translation scaling increases it becomes more likely that the haptic
  	// feedback loop at a surface will become "active" (smaller motions penetrating another object are sufficient to
  	// create forces ejecting the device's collision representation).  Therefore, a device that is having its
  	// translation scaled may required a force scaling filter to reduce the forces.
  	Vector3d force;
  	if (dataToFilter.vectors().get(DataStructures::Names::FORCE, &force))
  	{
  		force = m_transformInverse.linear() * force;
  		result->vectors().set(DataStructures::Names::FORCE, force);
  	}
  
  	Vector3d torque;
  	if (dataToFilter.vectors().get(DataStructures::Names::TORQUE, &torque))
  	{
  		torque = m_transformInverse.linear() * torque;
  		result->vectors().set(DataStructures::Names::TORQUE, torque);
  	}
  
  	// The Jacobians must be transformed into device space.  The Jacobians are scaled based on the translation scaling,
  	// so that the forces and torques displayed by the device will be correct for the scene-space motions, not for the
  	// device-space motions.  Let R be the linear rotation portion of our transform, s be the translation scaling
  	// factor, and J be the 3x3 upper-left corner of the spring Jacobian.  Then:
  	// delta-translation-in-scene = s * R * delta-translation-in-device
  	// delta-force-in-scene = J * delta-translation-in-scene
  	// So to transform J into the device space, we left-multiply by R^-1, and right-multiply by R.
  	// Then, because we want the forces to be scaled as in scene-space, we scale J by s.  The bottom-left 3x3 block
  	// in springJacobian (or damperJacobian) also transforms from delta-translation and is treated the same, while the
  	// two 3x3 blocks on the right half (of springJacobian or damperJacobian) will be multiplied by the delta-rotation
  	// (not delta-translation) and so should not be scaled.
  	DataGroup::DynamicMatrixType springJacobian;
  	if (dataToFilter.matrices().get(DataStructures::Names::SPRING_JACOBIAN, &springJacobian))
  	{
  		springJacobian.block<3,3>(0, 0).applyOnTheLeft(m_transformInverse.linear());
  		springJacobian.block<3,3>(0, 0).applyOnTheRight(m_transform.linear());
  		springJacobian.block<3,3>(3, 0).applyOnTheLeft(m_transformInverse.linear());
  		springJacobian.block<3,3>(3, 0).applyOnTheRight(m_transform.linear());
  		springJacobian.block<3,3>(0, 3).applyOnTheLeft(m_transformInverse.linear());
  		springJacobian.block<3,3>(0, 3).applyOnTheRight(m_transform.linear());
  		springJacobian.block<3,3>(3, 3).applyOnTheLeft(m_transformInverse.linear());
  		springJacobian.block<3,3>(3, 3).applyOnTheRight(m_transform.linear());
  		springJacobian.block<6,3>(0, 0) *= m_translationScale;
  		result->matrices().set(DataStructures::Names::SPRING_JACOBIAN, springJacobian);
  	}
  
  	RigidTransform3d inputPose;
  	if (dataToFilter.poses().get(DataStructures::Names::INPUT_POSE, &inputPose))
  	{
  		inputPose = m_transformInverse * inputPose;
  		inputPose.translation() /= m_translationScale;
  		result->poses().set(DataStructures::Names::INPUT_POSE, inputPose);
  	}
  
  	DataGroup::DynamicMatrixType damperJacobian;
  	if (dataToFilter.matrices().get(DataStructures::Names::DAMPER_JACOBIAN, &damperJacobian))
  	{
  		damperJacobian.block<3,3>(0, 0).applyOnTheLeft(m_transformInverse.linear());
  		damperJacobian.block<3,3>(0, 0).applyOnTheRight(m_transform.linear());
  		damperJacobian.block<3,3>(3, 0).applyOnTheLeft(m_transformInverse.linear());
  		damperJacobian.block<3,3>(3, 0).applyOnTheRight(m_transform.linear());
  		damperJacobian.block<3,3>(0, 3).applyOnTheLeft(m_transformInverse.linear());
  		damperJacobian.block<3,3>(0, 3).applyOnTheRight(m_transform.linear());
  		damperJacobian.block<3,3>(3, 3).applyOnTheLeft(m_transformInverse.linear());
  		damperJacobian.block<3,3>(3, 3).applyOnTheRight(m_transform.linear());
  		damperJacobian.block<6,3>(0, 0) *= m_translationScale;
  		result->matrices().set(DataStructures::Names::DAMPER_JACOBIAN, damperJacobian);
  	}
  
  	Vector3d inputLinearVelocity;
  	if (dataToFilter.vectors().get(DataStructures::Names::INPUT_LINEAR_VELOCITY, &inputLinearVelocity))
  	{
  		inputLinearVelocity = m_transformInverse.linear() * inputLinearVelocity;
  		inputLinearVelocity /= m_translationScale;
  		result->vectors().set(DataStructures::Names::INPUT_LINEAR_VELOCITY, inputLinearVelocity);
  	}
  
  	Vector3d inputAngularVelocity;
  	if (dataToFilter.vectors().get(DataStructures::Names::INPUT_ANGULAR_VELOCITY, &inputAngularVelocity))
  	{
  		inputAngularVelocity = m_transformInverse.linear() * inputAngularVelocity;
  		result->vectors().set(DataStructures::Names::INPUT_ANGULAR_VELOCITY, inputAngularVelocity);
  	}
  }
  
  double PoseTransform::getTranslationScale() const
  {
  	return m_translationScale;
  }
  
  void PoseTransform::setTranslationScale(double translationScale)
  {
  	boost::lock_guard<boost::mutex> lock(m_mutex);
  	m_translationScale = translationScale;
  }
  
  const RigidTransform3d& PoseTransform::getTransform() const
  {
  	return m_transform;
  }
  
  void PoseTransform::setTransform(const RigidTransform3d& transform)
  {
  	boost::lock_guard<boost::mutex> lock(m_mutex);
  	m_transform = transform;
  	m_transformInverse = m_transform.inverse();
  }
  
  };  // namespace Devices
  };  // namespace SurgSim
  