How to use

set

method

in

javax.vecmath.Tuple4d

private static void setVecmathFieldFromRosGeometryMessage(Message rosMessage, Packet<?> ihmcMessage, Method rosGetter, Field ihmcField,
   Class<?> ihmcMessageFieldType) throws IllegalAccessException, InvocationTargetException, InstantiationException
{
 if(ihmcMessageFieldType.equals(Point2d.class))
 {
   Point2d point2d = convertPoint2DRos((Point2dRosMessage) rosGetter.invoke(rosMessage));
   ihmcField.set(ihmcMessage, point2d);
 }
 else
 {
   if(rosGetter.getReturnType().equals(Quaternion.class))
   {
    Tuple4d newTuple = (Tuple4d) ihmcField.getType().newInstance();
    newTuple.set(convertQuaternion((Quaternion) rosGetter.invoke(rosMessage)));
    ihmcField.set(ihmcMessage, newTuple);
   }
   else if(rosGetter.getReturnType().equals(Vector3.class))
   {
    Tuple3d newTuple = (Tuple3d) ihmcField.getType().newInstance();
    newTuple.set(convertVector3((Vector3) rosGetter.invoke(rosMessage)));
    ihmcField.set(ihmcMessage, newTuple);
   }
 }
}