public static bool TestInternalObjects( ref IndexedMatrix trans0, ref IndexedMatrix trans1, ref IndexedVector3 delta_c, ref IndexedVector3 axis, ConvexPolyhedron convex0, ConvexPolyhedron convex1, float dmin) { float dp = delta_c.Dot(ref axis); IndexedVector3 localAxis0; InverseTransformPoint3x3(out localAxis0, ref axis,ref trans0); IndexedVector3 localAxis1; InverseTransformPoint3x3(out localAxis1, ref axis,ref trans1); IndexedVector3 p0; BoxSupport(ref convex0.m_extents, ref localAxis0, out p0); IndexedVector3 p1; BoxSupport(ref convex1.m_extents, ref localAxis1, out p1); float Radius0 = p0.X*localAxis0.X + p0.Y*localAxis0.Y + p0.Z*localAxis0.Z; float Radius1 = p1.X*localAxis1.X + p1.Y*localAxis1.Y + p1.Z*localAxis1.Z; float MinRadius = Radius0>convex0.m_radius ? Radius0 : convex0.m_radius; float MaxRadius = Radius1>convex1.m_radius ? Radius1 : convex1.m_radius; float MinMaxRadius = MaxRadius + MinRadius; float d0 = MinMaxRadius + dp; float d1 = MinMaxRadius - dp; float depth = d0<d1 ? d0:d1; if (depth > dmin) { return false; } return true; }
public static IndexedMatrix CreateLookAt(IndexedVector3 cameraPosition, IndexedVector3 cameraTarget, IndexedVector3 cameraUpVector) { IndexedVector3 vector3_1 = IndexedVector3.Normalize(cameraPosition - cameraTarget); IndexedVector3 vector3_2 = IndexedVector3.Normalize(IndexedVector3.Cross(cameraUpVector, vector3_1)); IndexedVector3 vector1 = IndexedVector3.Cross(vector3_1, vector3_2); IndexedMatrix matrix = IndexedMatrix.Identity; //matrix._basis = new IndexedBasisMatrix(vector3_2.X, vector1.X, vector3_1.X, vector3_2.Y, vector1.Y, vector3_1.Y, vector3_2.Z, vector1.Z, vector3_1.Z).Transpose(); matrix._basis = new IndexedBasisMatrix(ref vector3_2, ref vector1, ref vector3_1); matrix._origin = new IndexedVector3(-IndexedVector3.Dot(vector3_2, cameraPosition), -IndexedVector3.Dot(vector1, cameraPosition), -IndexedVector3.Dot(vector3_1, cameraPosition)); return(matrix); }
public static int ClipSegmentToLine(ClipVertex[] vOut, ClipVertex[] vIn, IndexedVector3 normal, float offset) { // Start with no output points int numOut = 0; // Calculate the distance of end points to the line float distance0 = normal.Dot(vIn[0].v) - offset; float distance1 = normal.Dot(vIn[1].v) - offset; // If the points are behind the plane if (distance0 <= 0.0f) vOut[numOut++] = vIn[0]; if (distance1 <= 0.0f) vOut[numOut++] = vIn[1]; // If the points are on different sides of the plane if (distance0 * distance1 < 0.0f) { // Find intersection point of edge and plane float interp = distance0 / (distance0 - distance1); vOut[numOut].v = vIn[0].v + interp * (vIn[1].v - vIn[0].v); if (distance0 > 0.0f) { vOut[numOut].id = vIn[0].id; } else { vOut[numOut].id = vIn[1].id; } ++numOut; } return numOut; }
public override IndexedVector3 LocalGetSupportingVertexWithoutMargin(ref IndexedVector3 dir) { IndexedVector3 dots = new IndexedVector3( dir.Dot(ref m_vertices1[0]), dir.Dot(ref m_vertices1[1]), dir.Dot(ref m_vertices1[2])); return m_vertices1[MathUtil.MaxAxis(ref dots)]; }
protected IndexedVector3 ComputeReflectionDirection(ref IndexedVector3 direction, ref IndexedVector3 normal) { return direction - (2.0f * direction.Dot(ref normal)) * normal; }
public static void bt_edge_plane(ref IndexedVector3 e1, ref IndexedVector3 e2, ref IndexedVector3 normal, out Vector4 plane) { IndexedVector3 planenormal = (e2-e1).Cross(ref normal); planenormal.Normalize(); plane = new Vector4(planenormal.ToVector3(), e2.Dot(ref planenormal)); }
public void ProjectionInterval(ref IndexedVector3 direction, out float vmin, out float vmax) { IndexedVector3 center = (m_max + m_min) * 0.5f; IndexedVector3 extend = m_max - center; float _fOrigin = direction.Dot(ref center); float _fMaximumExtent = extend.Dot(direction.Absolute()); vmin = _fOrigin - _fMaximumExtent; vmax = _fOrigin + _fMaximumExtent; }
private static void DLineClosestApproach(ref IndexedVector3 pa, ref IndexedVector3 ua, ref IndexedVector3 pb, ref IndexedVector3 ub, out float alpha, out float beta) { IndexedVector3 p = pb - pa; float uaub = ua.Dot(ref ub); float q1 = ua.Dot(ref p); float q2 = -(ub.Dot(ref p)); float d = 1 - uaub * uaub; if (d <= 0.0001f) { // @@@ this needs to be made more robust alpha = 0f; beta = 0f; } else { d = 1f / d; alpha = (q1 + uaub * q2) * d; beta = (uaub * q1 + q2) * d; } }
public float ComputeAngularImpulseDenominator(ref IndexedVector3 axis) { IndexedVector3 vec = axis * GetInvInertiaTensorWorld(); return axis.Dot(ref vec); }
public static float DistancePointPlane(ref IndexedVector4 plane, ref IndexedVector3 point) { return point.Dot(new IndexedVector3(plane.X, plane.Y, plane.Z)) - plane.W; }
public static float ComputeAngularImpulseDenominator(ref IndexedVector3 axis, ref IndexedBasisMatrix invInertiaWorld) { //IndexedVector3 vec = MathUtil.TransposeTransformNormal(axis, invInertiaWorld); IndexedVector3 vec = axis* invInertiaWorld; return axis.Dot(ref vec); }
///bilateral constraint between two dynamic objects ///positive distance = separation, negative distance = penetration public static void ResolveSingleBilateral(RigidBody body1, ref IndexedVector3 pos1, RigidBody body2, ref IndexedVector3 pos2, float distance, ref IndexedVector3 normal, ref float impulse, float timeStep) { float normalLenSqr = normal.LengthSquared(); Debug.Assert(Math.Abs(normalLenSqr) < 1.1f); if (normalLenSqr > 1.1f) { impulse = 0f; return; } IndexedVector3 rel_pos1 = pos1 - body1.GetCenterOfMassPosition(); IndexedVector3 rel_pos2 = pos2 - body2.GetCenterOfMassPosition(); //this jacobian entry could be re-used for all iterations IndexedVector3 vel1 = body1.GetVelocityInLocalPoint(ref rel_pos1); IndexedVector3 vel2 = body2.GetVelocityInLocalPoint(ref rel_pos2); IndexedVector3 vel = vel1 - vel2; IndexedBasisMatrix m1 = body1.GetCenterOfMassTransform()._basis.Transpose(); IndexedBasisMatrix m2 = body2.GetCenterOfMassTransform()._basis.Transpose(); JacobianEntry jac = new JacobianEntry(m1, m2, rel_pos1, rel_pos2, normal, body1.GetInvInertiaDiagLocal(), body1.GetInvMass(), body2.GetInvInertiaDiagLocal(), body2.GetInvMass()); float jacDiagAB = jac.GetDiagonal(); float jacDiagABInv = 1f / jacDiagAB; float rel_vel = jac.GetRelativeVelocity( body1.GetLinearVelocity(), body1.GetCenterOfMassTransform()._basis.Transpose() * body1.GetAngularVelocity(), body2.GetLinearVelocity(), body2.GetCenterOfMassTransform()._basis.Transpose() * body2.GetAngularVelocity()); float a = jacDiagABInv; rel_vel = normal.Dot(ref vel); //todo: move this into proper structure float contactDamping = 0.2f; if (ONLY_USE_LINEAR_MASS) { float massTerm = 1f / (body1.GetInvMass() + body2.GetInvMass()); impulse = -contactDamping * rel_vel * massTerm; } else { float velocityImpulse = -contactDamping * rel_vel * jacDiagABInv; impulse = velocityImpulse; } }