import logging; logger = logging.getLogger("morse." + __name__)
from morse.builder.data import MORSE_COMPONENTS
from morse.core import mathutils
import morse.core.sensor
from morse.helpers.components import add_data, add_property
from morse.helpers.coordinates import CoordinateConverter
from math import degrees
import datetime
import os
import numpy
def _decimal_date(date):
bisextile = (date.year % 4 == 0 and date.year % 100 != 0) or (date.year % 400 == 0)
days_month = [0, 31, 28, 31, 30, 31, 30, 31, 31, 30, 31, 30, 31]
if bisextile:
days_month[2] += 1
day_nb = date.day
for i in range(0, date.month):
day_nb += days_month[i]
return date.year + (day_nb - 1.0) / (365.0 + bisextile)
[docs]class MagnetoDriver(object):
def __init__(self, date = None):
from morse.sensors._magnetometer import Magnetometer as Mag
self._mag = Mag(os.path.join(MORSE_COMPONENTS, 'WMM.COF'))
self._coord_conv = CoordinateConverter.instance()
if date:
self._date = date
else:
self._date = _decimal_date(datetime.date.today())
[docs] def compute(self, pose):
pos = numpy.matrix(pose.translation)
pos_ltp = self._coord_conv.blender_to_ltp(pos)
pos_lla = self._coord_conv.ltp_to_geodetic(pos_ltp)
(decl, incl, f, h, x, y, z) = self._mag.compute(
degrees(pos_lla[0, 0]),
degrees(pos_lla[0, 1]),
pos_lla[0, 2] / 1000.0, self._date)
mag_field = mathutils.Vector((x, y, z))
return mag_field * pose.rotation_matrix
[docs]class Magnetometer(morse.core.sensor.Sensor):
"""
This sensor computes the magnetic field vector, at the sensor
position. It relies on the WMM2015 model, available
`at NOAA <http://www.ngdc.noaa.gov/geomag/WMM/DoDWMM.shtml>`_.
.. warning::
For proper computation, the sensor needs a real position on
Earth, and so the following properties should be added at the
environment level:
- **longitude** in degrees (double) of Blender origin
- **latitude** in degrees (double) of Blender origin
- **altitude** in m of the Blender origin
- optionnaly **angle_against_north** in degrees is the angle
between geographic north and the blender X axis.
**angle_against_north** is positive when the blender X-axis is
east of true north, and negative when it is to the west.
"""
_name = "Magnetometer"
add_data('x', 0.0, "float",
'Northern component of the magnetic field vector, in nT')
add_data('y', 0.0, "float",
'Eastern component of the magnetic field vector, in nT')
add_data('z', 0.0, "float",
'Downward component of the magnetic field vector, in nT')
add_property('date', None, 'date', 'float', 'the date used to adjust \
for magnetic field. If not precised, consider the today \
date')
def __init__(self, obj, parent=None):
""" Constructor method.
Receives the reference to the Blender object.
The second parameter should be the name of the object's parent.
"""
logger.info('%s initialization' % obj.name)
# Call the constructor of the parent class
morse.core.sensor.Sensor.__init__(self, obj, parent)
self._mag = MagnetoDriver(self.date)
logger.info('Component initialized, runs at %.2f Hz', self.frequency)
[docs] def default_action(self):
mag_field = self._mag.compute(self.position_3d)
self.local_data['x'] = mag_field[0]
self.local_data['y'] = mag_field[1]
self.local_data['z'] = mag_field[2]