def cps(mat_1, mat_2):
"""
Find rotation and translation difference between two transformations.
Arguments:
*mat_1,mat_2*
Transformation matrices.
Returns:
The translation and rotation differences
"""
mat1 = matrix(mat_1)
mat2 = matrix(mat_2)
# mat to euler (angular distance not accurate!)
#t1,p1,s1 = _rotmat_to_euler(mat1)
#t2,p2,s2 = _rotmat_to_euler(mat2)
#ang_magnitude = msqrt(mpow(t2-t1,2)+mpow(p2-p1,2)+mpow(s2-s1,2))
matR1 = matrix([mat_1[0][:-1], mat_1[1][:-1], mat_1[2][:-1]])
matR2 = matrix([mat_2[0][:-1], mat_2[1][:-1], mat_2[2][:-1]])
matR = matR2 * matR1.transpose()
ang_magnitude, xV, yV, zV = _rotmat_to_axisangle(matR)
ang_magnitude = ang_magnitude * (180.0 / pi)
shift = msqrt(
mpow(mat2[0, 3] - mat1[0, 3], 2) + mpow(mat2[1, 3] - mat1[1, 3], 2) +
mpow(mat2[2, 3] - mat1[2, 3], 2))
#print (mpow(mat2[0,3]-mat1[0,3],2)+mpow(mat2[1,3]-mat1[1,3],2)+mpow(mat2[2,3]-mat1[2,3],2)), ang_magnitude
#acps_score = (pi/360)*(mpow(mat2[0,3]-mat1[0,3],2)+mpow(mat2[1,3]-mat1[1,3],2)+mpow(mat2[2,3]-mat1[2,3],2))*abs(ang_magnitude)
return shift, ang_magnitude
def is_close_phy(self, coor1, coor2, max_dist=15):
self.etc.log("Calculating proximity(PHY)")
try:
dX = coor1[0] - coor2[0]
dY = coor1[1] - coor2[1]
dist = msqrt(mpow(dX, 2) + mpow(dY, 2))
return (dist < max_dist)
except Exception as e:
self.etc.log(e)
def _calc_distance(self, v1_la, v2_la, v1_lo, v2_lo):
''' Calculates distance with Haversine Formula '''
lat1 = radians(v1_la)
lat2 = radians(v2_la)
delta_lat = radians(v2_la - v1_la)
delta_lon = radians(v2_lo - v1_lo)
a = ( mpow(sin(delta_lat/2), 2) + cos(lat1) * cos(lat2) *
mpow(sin(delta_lon/2), 2) )
c = 2 * atan2(sqrt(a), sqrt(1-a))
return c * EARTH_RADIUS
def levy_flight(index_beta):
exponent = 1 / index_beta
numerator = gamma(1 + index_beta) * sin((pi * index_beta) / 2)
denominator = gamma(
(1 + index_beta) / 2) * index_beta * mpow(2, (index_beta - 1) / 2)
base = mpow(numerator / denominator, exponent)
base = mpow(base, 2)
u = np.random.normal(0, base)
v = abs(np.random.normal(0, 1))
random_step = u / mpow(v, 1 / index_beta)
return random_step
def calculateMortgageAnnuityPayment(self,
full_price=0,
years=0,
initial_fee=0,
mortgage_percentage=Decimal(9.4)):
from math import pow as mpow
price = full_price - initial_fee
months = years * 12
monthly_mortgage_proportion = mortgage_percentage / 1200
monthly_payment = price * monthly_mortgage_proportion / \
(1 - Decimal(mpow(1 + monthly_mortgage_proportion, -months)))
return {
'monthly_payment': monthly_payment,
'mortgage_percentage': mortgage_percentage,
'amount_of_mortgage': price,
}
def sum_log_probabilities(next_logP, logP):
if next_logP == 0: return True, logP
if next_logP > logP:
common = next_logP
diff_exponent = logP - common
else:
common = logP
diff_exponent = next_logP - common
logP = common + ( (log(1 + mpow(10, diff_exponent))) / log(10) )
# The cumulative summation stops when the increasing is less than 10e-4
if next_logP - logP > -4: return True, logP
return False, logP
def get_ego_displ(vdy_data, cycle_time):
""" Calculate the EGO displacement for each cycle
:param vdy_data: Vehicle Dynamic data (dictionary from data_extractor)
:type vdy_data: dict
:param cycle_time: Time of each cycle
:type cycle_time: list
:return: ego displacement vector
"""
velocity = vdy_data[PORT_VDY_VEHICLE_SPEED]
accel = vdy_data[PORT_VDY_VEHICLE_ACCEL]
ego_displ = [0] * len(velocity)
for cycle_idx in range(0, len(velocity)):
delta_t = cycle_time[cycle_idx]
ego_displ[cycle_idx] = (velocity[cycle_idx] * delta_t) + (
0.5 * accel[cycle_idx] * mpow(delta_t, 2))
return ego_displ
def uncompand(A):
""" Uncompand an sRGB byte value to a linearized value """
# q.v. http://www.brucelindbloom.com/index.html?Eqn_RGB_to_XYZ.html
V = A / 255.0
return (V <= 0.04045) and (V / 12.92) or mpow(((V + 0.055) / 1.055), 2.4)
def compand(v):
""" Compand a linearized value to an sRGB byte value """
# q.v. http://www.brucelindbloom.com/index.html?Math.html
V = (v <= 0.0031308) and (v * 12.92) or fabs((1.055 * mpow(v, 1 / 2.4)) -
0.055)
return int(V * 255.0)
def distance(self, c: "Coord") -> float:
return sqrt(
mpow(self.x - c.x, 2) + mpow(self.y - c.y, 2) +
mpow(self.z - c.z, 2))
def math_pow(): for _ in range(100000000): a = mpow(10., 2)
def moveEvent(self, dx=0, dy=0, free=False):
"""
Generate move events from parametters and displacement
@param int dx delta movement from last call on x axis
@param int dy delta movement from last call on y axis
@param bool free set to true for free ball move
@return float absolute distance moved this tick
"""
# Compute time step
_tmp = time.time()
dt = _tmp - self._lastTime
self._lastTime = _tmp
if not free:
# Compute mouse mouvement from interger part of d * scale
self._dx += dx * self._xscale
self._dy += dy * self._yscale
self.relEvent(rel=Rels.REL_X, val=int(self._dx))
self.relEvent(rel=Rels.REL_Y, val=int(self._dy))
self.synEvent()
# Remove
self._dx -= int(self._dx)
self._dy -= int(self._dy)
# Compute instant velocity
self._xvel = dx * self._radscale / dt
self._yvel = dy * self._radscale / dt
else:
# Free movement update velocity and compute movement
# Cap friction desceleration
_dvx = min(abs(self._xvel), self._a * dt)
_dvy = min(abs(self._yvel), self._a * dt)
# compute new velocity
_xvel = self._xvel - copysign(_dvx, self._xvel)
_yvel = self._yvel - copysign(_dvy, self._yvel)
# compute displacement
dx = (((_xvel + self._xvel) / 2) * dt) / self._radscale
dy = (((_yvel + self._yvel) / 2) * dt) / self._radscale
self._xvel = _xvel
self._yvel = _yvel
self._dx += dx * self._xscale
self._dy += dy * self._yscale
self.relEvent(rel=Rels.REL_X, val=int(self._dx))
self.relEvent(rel=Rels.REL_Y, val=int(self._dy))
self.synEvent()
# Remove
self._dx -= int(self._dx)
self._dy -= int(self._dy)
return sqrt(mpow(dx, 2) + mpow(dy, 2))
