Exemplo n.º 1
0
        public override void AccumulateCorrection(ref VRageMath.Vector3 correction, ref float weight)
        {
            Vector3D position = Parent.PositionAndOrientation.Translation;

            BoundingBoxD box = new BoundingBoxD(position - Vector3D.One, position + Vector3D.One);

            Vector3D currentMovement = Parent.ForwardVector * Parent.Speed;
            if (Parent.Speed > 0.01f)
                currentMovement.Normalize();

            // Don't do any correction if we're not moving
            if (currentMovement.LengthSquared() < 0.01)
            {
                return;
            }

            var entities = MyEntities.GetEntitiesInAABB(ref box);
            foreach (var entity in entities)
            {
                var environmentItems = entity as MyEnvironmentItems;
                if (environmentItems == null) continue;

                environmentItems.GetItemsInRadius(ref position, 6.0f, m_trees);

                foreach (var item in m_trees)
                {
                    Vector3D dir = item - position;
                    var dist = dir.Normalize();
                    dir = Vector3D.Reject(currentMovement, dir);

                    dir.Y = 0.0f;
                    if (dir.Z * dir.Z + dir.X * dir.X < 0.1)
                    {
                        Vector3D dirLocal = Vector3D.TransformNormal(dir, Parent.PositionAndOrientationInverted);
                        dir = position - item;
                        dir = Vector3D.Cross(Vector3D.Up, dir);
                        if (dirLocal.X < 0)
                            dir = -dir;
                    }

                    dir.Normalize();

                    correction += (6f - dist) * Weight * dir;
                    if (!correction.IsValid())
                    {
                        System.Diagnostics.Debugger.Break();
                    }
                }
                m_trees.Clear();
            }

            weight += Weight;

            entities.Clear();
        }
Exemplo n.º 2
0
        public override void AccumulateCorrection(ref VRageMath.Vector3 correction, ref float weight)
        {
            // Don't do any correction if we're not moving
            if (Parent.Speed < 0.01)
                return;

            Vector3D position = Parent.PositionAndOrientation.Translation;

            // Find trees
            Quaternion rotation = Quaternion.Identity;
            HkShape sphereShape = new HkSphereShape(6);
            MyPhysics.GetPenetrationsShape(sphereShape, ref position, ref rotation, m_trees, MyPhysics.CollisionLayers.NoVoxelCollisionLayer);

            foreach (var tree in m_trees)
            {
                // Make sure the tree is actually a tree
                if (tree.Body == null) continue;
                MyPhysicsBody physicsBody = tree.Body.UserObject as MyPhysicsBody;
                if (physicsBody == null) continue;

                HkShape bodyShape = tree.Body.GetShape();
                if (bodyShape.ShapeType != HkShapeType.StaticCompound)
                    continue;

                // Get the static compound shape
                HkStaticCompoundShape staticCompoundShape = (HkStaticCompoundShape)bodyShape;

                int instanceId;
                uint childKey;
                staticCompoundShape.DecomposeShapeKey(tree.ShapeKey, out instanceId, out childKey);

                // Get the local shape position, and add entity world position
                Vector3D item = staticCompoundShape.GetInstanceTransform(instanceId).Translation;
                item += physicsBody.GetWorldMatrix().Translation;

                // Avoid tree
                Vector3D dir = item - position;
                var dist = dir.Normalize();
                dir = Vector3D.Reject(Parent.ForwardVector, dir);

                dir.Y = 0.0f;
                if (dir.Z * dir.Z + dir.X * dir.X < 0.1)
                {
                    Vector3D dirLocal = Vector3D.TransformNormal(dir, Parent.PositionAndOrientationInverted);
                    dir = position - item;
                    dir = Vector3D.Cross(Vector3D.Up, dir);
                    if (dirLocal.X < 0)
                        dir = -dir;
                }

                dir.Normalize();

                correction += (6f - dist) * Weight * dir;
                if (!correction.IsValid())
                {
                    System.Diagnostics.Debugger.Break();
                }
            }

            m_trees.Clear();

            weight += Weight;
        }