TODO_ERIN might not need to return the separation
(cache *B2SimplexCache, proxyA *B2DistanceProxy, sweepA B2Sweep, proxyB *B2DistanceProxy, sweepB B2Sweep, t1 float64)
| 76 | |
| 77 | // TODO_ERIN might not need to return the separation |
| 78 | func (sepfunc *B2SeparationFunction) Initialize(cache *B2SimplexCache, proxyA *B2DistanceProxy, sweepA B2Sweep, proxyB *B2DistanceProxy, sweepB B2Sweep, t1 float64) float64 { |
| 79 | |
| 80 | sepfunc.M_proxyA = proxyA |
| 81 | sepfunc.M_proxyB = proxyB |
| 82 | count := cache.Count |
| 83 | B2Assert(0 < count && count < 3) |
| 84 | |
| 85 | sepfunc.M_sweepA = sweepA |
| 86 | sepfunc.M_sweepB = sweepB |
| 87 | |
| 88 | xfA := MakeB2Transform() |
| 89 | xfB := MakeB2Transform() |
| 90 | sepfunc.M_sweepA.GetTransform(&xfA, t1) |
| 91 | sepfunc.M_sweepB.GetTransform(&xfB, t1) |
| 92 | |
| 93 | if count == 1 { |
| 94 | sepfunc.M_type = B2SeparationFunction_Type.E_points |
| 95 | localPointA := sepfunc.M_proxyA.GetVertex(cache.IndexA[0]) |
| 96 | localPointB := sepfunc.M_proxyB.GetVertex(cache.IndexB[0]) |
| 97 | pointA := B2TransformVec2Mul(xfA, localPointA) |
| 98 | pointB := B2TransformVec2Mul(xfB, localPointB) |
| 99 | sepfunc.M_axis = B2Vec2Sub(pointB, pointA) |
| 100 | s := sepfunc.M_axis.Normalize() |
| 101 | return s |
| 102 | } else if cache.IndexA[0] == cache.IndexA[1] { |
| 103 | // Two points on B and one on A. |
| 104 | sepfunc.M_type = B2SeparationFunction_Type.E_faceB |
| 105 | localPointB1 := proxyB.GetVertex(cache.IndexB[0]) |
| 106 | localPointB2 := proxyB.GetVertex(cache.IndexB[1]) |
| 107 | |
| 108 | sepfunc.M_axis = B2Vec2CrossVectorScalar( |
| 109 | B2Vec2Sub(localPointB2, localPointB1), |
| 110 | 1.0, |
| 111 | ) |
| 112 | |
| 113 | sepfunc.M_axis.Normalize() |
| 114 | normal := B2RotVec2Mul(xfB.Q, sepfunc.M_axis) |
| 115 | |
| 116 | sepfunc.M_localPoint = B2Vec2MulScalar(0.5, B2Vec2Add(localPointB1, localPointB2)) |
| 117 | pointB := B2TransformVec2Mul(xfB, sepfunc.M_localPoint) |
| 118 | |
| 119 | localPointA := proxyA.GetVertex(cache.IndexA[0]) |
| 120 | pointA := B2TransformVec2Mul(xfA, localPointA) |
| 121 | |
| 122 | s := B2Vec2Dot(B2Vec2Sub(pointA, pointB), normal) |
| 123 | if s < 0.0 { |
| 124 | sepfunc.M_axis = sepfunc.M_axis.OperatorNegate() |
| 125 | s = -s |
| 126 | } |
| 127 | |
| 128 | return s |
| 129 | } else { |
| 130 | // Two points on A and one or two points on B. |
| 131 | sepfunc.M_type = B2SeparationFunction_Type.E_faceA |
| 132 | localPointA1 := sepfunc.M_proxyA.GetVertex(cache.IndexA[0]) |
| 133 | localPointA2 := sepfunc.M_proxyA.GetVertex(cache.IndexA[1]) |
| 134 | |
| 135 | sepfunc.M_axis = B2Vec2CrossVectorScalar(B2Vec2Sub(localPointA2, localPointA1), 1.0) |
no test coverage detected