mathCommon.py (11147B)
1 #Embedded file name: C:\BuildAgent\work\20d73f1b24eed06e\eve\release\V24.01\carbon\common\script\util\mathCommon.py 2 from carbon.common.lib.const import FLOAT_TOLERANCE 3 from carbon.common.script.util.mathErrors import InvalidInput 4 import logging 5 import math 6 import random 7 try: 8 import geo2 9 except ImportError: 10 geo2 = None 11 12 log = logging.getLogger(__name__) 13 14 def ConvertGeoToTriMatrix(geoMatrix): 15 return ConvertTupleToTriMatrix(geoMatrix) 16 17 18 def ConvertTriToGeoMatrix(triMatrix): 19 return ConvertTriToTupleMatrix(triMatrix) 20 21 22 def ConvertTupleToTriMatrix(tupleMatrix): 23 import trinity 24 return trinity.TriMatrix(tupleMatrix[0][0], tupleMatrix[0][1], tupleMatrix[0][2], tupleMatrix[0][3], tupleMatrix[1][0], tupleMatrix[1][1], tupleMatrix[1][2], tupleMatrix[1][3], tupleMatrix[2][0], tupleMatrix[2][1], tupleMatrix[2][2], tupleMatrix[2][3], tupleMatrix[3][0], tupleMatrix[3][1], tupleMatrix[3][2], tupleMatrix[3][3]) 25 26 27 def ConvertTriToTupleMatrix(triMatrix): 28 return ((triMatrix._11, 29 triMatrix._12, 30 triMatrix._13, 31 triMatrix._14), 32 (triMatrix._21, 33 triMatrix._22, 34 triMatrix._23, 35 triMatrix._24), 36 (triMatrix._31, 37 triMatrix._32, 38 triMatrix._33, 39 triMatrix._34), 40 (triMatrix._41, 41 triMatrix._42, 42 triMatrix._43, 43 triMatrix._44)) 44 45 46 def FloatCloseEnough(a, b, epsilon = None): 47 if not epsilon: 48 epsilon = FLOAT_TOLERANCE 49 return abs(a - b) < epsilon 50 51 52 def FloatLTE(a, b, epsilon = None): 53 return a < b or FloatCloseEnough(a, b, epsilon) 54 55 56 def FloatGTE(a, b, epsilon = None): 57 return a > b or FloatCloseEnough(a, b, epsilon) 58 59 60 def VectorCloseEnough(a, b): 61 for i in range(len(a)): 62 if not FloatCloseEnough(a[i], b[i]): 63 return False 64 65 return True 66 67 68 def RandomFloatBetween(a, b): 69 return a + (b - a) * random.random() 70 71 72 def CreateOrthogonalVector(v): 73 minIdx = v.index(min(v)) 74 maxIdx = v.index(max(v)) 75 ortho = list(v) 76 ortho[minIdx], ortho[maxIdx] = -ortho[maxIdx], ortho[minIdx] 77 return tuple(ortho) 78 79 80 def GetRandomHemisphereVector(axis): 81 radius = geo2.Vec3Length(axis) 82 randVec = RandomVector(radius) 83 dotProduct = geo2.Vec3Dot(randVec, axis) 84 if dotProduct <= 0: 85 randVec = list(randVec) 86 for i in xrange(len(randVec)): 87 if abs(axis[i] - randVec[i]) > radius: 88 randVec[i] *= -1 89 90 randVec = tuple(randVec) 91 return randVec 92 93 94 def RandomVector(radius = 1.0, width = 0.0): 95 t = random.random() * 2.0 * math.pi 96 u = (random.random() - 0.5) * 2.0 97 sq = math.sqrt(1.0 - u * u) 98 if width > 0.0: 99 if width > radius: 100 width = radius 101 radius = radius - random.random() * width 102 x = radius * sq * math.cos(t) 103 y = radius * sq * math.sin(t) 104 z = radius * u 105 return [x, y, z] 106 107 108 def TrinityMatrixCloseEnough(a, b): 109 return FloatCloseEnough(a._11, b._11) and FloatCloseEnough(a._21, b._21) and FloatCloseEnough(a._31, b._31) and FloatCloseEnough(a._41, b._41) and FloatCloseEnough(a._12, b._12) and FloatCloseEnough(a._22, b._22) and FloatCloseEnough(a._32, b._32) and FloatCloseEnough(a._42, b._42) and FloatCloseEnough(a._13, b._13) and FloatCloseEnough(a._23, b._23) and FloatCloseEnough(a._33, b._33) and FloatCloseEnough(a._43, b._43) and FloatCloseEnough(a._14, b._14) and FloatCloseEnough(a._24, b._24) and FloatCloseEnough(a._34, b._34) and FloatCloseEnough(a._44, b._44) 110 111 112 def MatrixCloseEnough(a, b): 113 for i in xrange(0, 4): 114 for j in xrange(0, 4): 115 if not FloatCloseEnough(a[i][j], b[i][j]): 116 return False 117 118 return True 119 120 121 def RotateQuatByYaw(oldRot, yawAngle): 122 newRot = (0, 123 math.sin(yawAngle / 2.0), 124 0, 125 math.cos(yawAngle / 2.0)) 126 return geo2.QuaternionMultiply(oldRot, newRot) 127 128 129 def CalculateShortestRotation(angle): 130 angle = math.fmod(angle, 2 * math.pi) 131 if angle <= -math.pi: 132 angle += math.pi * 2 133 elif angle > math.pi: 134 angle -= math.pi * 2 135 return angle 136 137 138 def BoundedRotateQuatByYaw(entityRotation, facingAngle, maxRotate): 139 if abs(facingAngle) < 0.01: 140 return entityRotation 141 maxRotate = abs(maxRotate) 142 facingAngle = CalculateShortestRotation(facingAngle) 143 if maxRotate < abs(facingAngle): 144 if facingAngle < 0: 145 facingAngle = -maxRotate 146 else: 147 facingAngle = maxRotate 148 return RotateQuatByYaw(entityRotation, facingAngle) 149 150 151 def GetLesserAngleBetweenYaws(first, second): 152 return CalculateShortestRotation(second - first) 153 154 155 def GetDeltaAngleToFaceTarget(sourcePos, sourceRot, targetPos): 156 targetVec = geo2.Vec3Subtract(targetPos, sourcePos) 157 theta = GetYawAngleFromDirectionVector(targetVec) 158 yaw, trash, trash = geo2.QuaternionRotationGetYawPitchRoll(sourceRot) 159 return GetLesserAngleBetweenYaws(yaw, theta) 160 161 162 def CreateDirectionVectorFromYawAngle(yaw): 163 return (math.sin(yaw), 0.0, math.cos(yaw)) 164 165 166 def CreateDirectionVectorFromYawAndPitchAngle(yaw, pitch): 167 direction = (-math.sin(pitch) * math.cos(yaw), math.cos(pitch), -math.sin(pitch) * math.sin(yaw)) 168 return direction 169 170 171 def GetYawAngleFromDirectionVector(vec3): 172 testVec = list(vec3) 173 testVec[1] = 0.0 174 testVec = list(geo2.Vec3Normalize(testVec)) 175 if testVec[2] >= 1.0: 176 testVec[2] = 1.0 177 elif testVec[2] <= -1.0: 178 testVec[2] = -1.0 179 yaw = math.acos(testVec[2]) 180 if testVec[0] < 0.0: 181 yaw = math.pi * 2 - yaw 182 return yaw 183 184 185 def GetPitchAngleFromDirectionVector(vec3): 186 testVec = list(geo2.Vec3Normalize(vec3)) 187 if testVec[1] >= 1.0: 188 testVec[1] = 1.0 189 elif testVec[1] <= -1.0: 190 testVec[1] = -1.0 191 pitch = math.asin(testVec[1]) 192 return pitch 193 194 195 def GetNewQuatToFacePos(sourcePos, sourceRot, targetPos): 196 deltaAngle = GetDeltaAngleToFaceTarget(sourcePos, sourceRot, targetPos) 197 yaw, pitch, roll = geo2.QuaternionRotationGetYawPitchRoll(sourceRot) 198 yaw += deltaAngle 199 return geo2.QuaternionRotationSetYawPitchRoll(yaw, pitch, roll) 200 201 202 def CalcLinearIntpValue(startVal, targetVal, startTime, curTime, howLong): 203 endTime = startTime + howLong 204 if curTime > endTime: 205 return targetVal 206 elif startTime > curTime: 207 return startVal 208 else: 209 timeFactor = max(1.0 * (curTime - startTime) / howLong, 0) 210 return startVal + timeFactor * (targetVal - startVal) 211 212 213 def CalcScaledValueOverInterval(startVal, targetVal, intervalLength, intervalRemaining, minFactor = None, maxFactor = None): 214 if intervalLength == 0: 215 return targetVal 216 factor = 1.0 - 1.0 * intervalRemaining / intervalLength 217 if minFactor: 218 factor = max(factor, minFactor) 219 if maxFactor: 220 factor = min(factor, maxFactor) 221 return startVal + (targetVal - startVal) * factor 222 223 224 def LineIntersectVerticalPlaneWithTwoPoints(lineV1, lineV2, planeV1, planeV2): 225 v1 = planeV1 226 v2 = planeV2 227 v3 = geo2.Vector(*planeV1) 228 v3.y += 1 229 plane = geo2.PlaneFromPoints(v1, v2, v3) 230 intersectionPoint = geo2.PlaneIntersectLine(plane, lineV1, lineV2) 231 return intersectionPoint 232 233 234 def DecomposeMatrix(matrix): 235 if hasattr(matrix, '__bluetype__') and matrix.__bluetype__ == 'trinity.TriMatrix': 236 matrix = util.ConvertTriToTupleMatrix(matrix) 237 scale, rot, pos = geo2.MatrixDecompose(matrix) 238 rot = geo2.QuaternionRotationGetYawPitchRoll(rot) 239 return (pos, rot, scale) 240 241 242 def ComposeMatrix(pos, rot, scale): 243 return geo2.MatrixTransformation(None, None, scale, None, rot, pos) 244 245 246 def GetViewFrustumPlanes(viewMatrix, projectionMatrix): 247 viewProjectionMatrix = geo2.MatrixMultiply(viewMatrix, projectionMatrix) 248 left = tuple((viewProjectionMatrix[i][0] + viewProjectionMatrix[i][3] for i in xrange(0, 4))) 249 right = tuple((-viewProjectionMatrix[i][0] + viewProjectionMatrix[i][3] for i in xrange(0, 4))) 250 bottom = tuple((viewProjectionMatrix[i][1] + viewProjectionMatrix[i][3] for i in xrange(0, 4))) 251 top = tuple((-viewProjectionMatrix[i][1] + viewProjectionMatrix[i][3] for i in xrange(0, 4))) 252 near = tuple((viewProjectionMatrix[i][2] + viewProjectionMatrix[i][3] for i in xrange(0, 4))) 253 far = tuple((-viewProjectionMatrix[i][2] + viewProjectionMatrix[i][3] for i in xrange(0, 4))) 254 return [left, 255 right, 256 bottom, 257 top, 258 near, 259 far] 260 261 262 def GetSignedDistanceFromPointToPlane(point, plane): 263 x0, y0, z0 = point 264 A, B, C, D = plane 265 numerator = A * x0 + B * y0 + C * z0 + D 266 denominator = math.sqrt(A * A + B * B + C * C) 267 if FloatCloseEnough(denominator, 0.0): 268 raise InvalidInput('Cannot calculate distance as the provided plane equation is not valid (all 0s)') 269 return numerator / denominator 270 271 272 def IsWithinFrustum(point, viewMatrix, projectionMatrix): 273 planes = GetViewFrustumPlanes(viewMatrix, projectionMatrix) 274 for plane in planes: 275 try: 276 if GetSignedDistanceFromPointToPlane(point, plane) < 0: 277 return False 278 except InvalidInput: 279 continue 280 281 return True 282 283 284 def GetYawAndPitchAnglesRad(position0, position1): 285 quat = GetYawAndPitchQuaternion(position0, position1) 286 rot = geo2.QuaternionRotationGetYawPitchRoll(quat) 287 return (rot[0], rot[1]) 288 289 290 def GetYawAndPitchQuaternion(position0, position1): 291 direction = geo2.Vec3Subtract(position1, position0) 292 direction = geo2.Vec3Normalize(direction) 293 quat = geo2.QuaternionRotationArc(direction, (0.0, 0.0, 1.0)) 294 return quat 295 296 297 def GetYawAndPitchAnglesDeg(position0, position1): 298 yaw, pitch = GetYawAndPitchAnglesRad(position0, position1) 299 yaw = yaw * 180 / math.pi 300 pitch = pitch * 180 / math.pi 301 return (yaw, pitch) 302 303 304 def GetAngleBetweenTwoVectors(v1, v2): 305 try: 306 side1 = geo2.Vec3Length(v1) 307 side2 = geo2.Vec3Length(v2) 308 if side1 >= side2: 309 a = side1 310 b = side2 311 c = geo2.Vec3Length(geo2.Vec3Subtract(v1, v2)) 312 else: 313 a = side2 314 b = side1 315 c = geo2.Vec3Length(geo2.Vec3Subtract(v2, v1)) 316 mu = c - (a - b) if b >= c else b - (a - c) 317 numerator = (a - b + c) * mu 318 denominator = (a + (b + c)) * (a - c + b) 319 if FloatCloseEnough(denominator, 0.0): 320 if FloatCloseEnough(numerator, 0.0): 321 raise InvalidInput('Cannot calculate angle for degenerate triangle (encountering atan(0/0) = NaN)') 322 if numerator > 0.0: 323 return math.pi 324 raise RuntimeError('Cannot calculate angle as these side lengths do not belong to a real triangle') 325 angle = 2.0 * math.atan(math.sqrt(numerator / denominator)) 326 return angle 327 except Exception as exc: 328 log.exception('Failed to calculate angle between 2 vectors: %s and %s. Exception: %s', v1, v2, exc) 329 raise exc 330 331 332 def ProjectPointToLineSegment(A, B, p): 333 AB = geo2.Vec3SubtractD(B, A) 334 AB_squared = geo2.Vec3DotD(AB, AB) 335 if AB_squared == 0: 336 return A 337 Ap = geo2.Vec3SubtractD(p, A) 338 t = geo2.Vec3DotD(Ap, AB) / AB_squared 339 if t < 0.0: 340 return A 341 elif t > 1.0: 342 return B 343 else: 344 return geo2.Vec3AddD(A, geo2.Vec3ScaleD(AB, t))