C# IndexedVector3.Dot方法代码示例

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 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;
		}

		protected IndexedVector3 ComputeReflectionDirection(ref IndexedVector3 direction, ref IndexedVector3 normal)
		{
            return direction - (2.0f * direction.Dot(ref normal)) * normal;
		}

 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)];
 }

        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;
        }

 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));
 }

        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;
			}
		}