Java Code Examples for com.jme3.math.Vector3f#length()

/**
 * The method updates the geometry according to the poitions of the bones.
 */
public void updateGeometry() {
    VertexBuffer vb = this.getBuffer(Type.Position);
    FloatBuffer posBuf = this.getFloatBuffer(Type.Position);
    posBuf.clear();
    for (int i = 0; i < skeleton.getBoneCount(); ++i) {
        Bone bone = skeleton.getBone(i);
        Vector3f parentTail = bone.getModelSpacePosition().add(bone.getModelSpaceRotation().mult(Vector3f.UNIT_Y.mult(boneLengths.get(i))));

        for (Bone child : bone.getChildren()) {
            Vector3f childHead = child.getModelSpacePosition();
            Vector3f v = childHead.subtract(parentTail);
            float pointDelta = v.length() / POINT_AMOUNT;
            v.normalizeLocal().multLocal(pointDelta);
            Vector3f pointPosition = parentTail.clone();
            for (int j = 0; j < POINT_AMOUNT; ++j) {
                posBuf.put(pointPosition.getX()).put(pointPosition.getY()).put(pointPosition.getZ());
                pointPosition.addLocal(v);
            }
        }
    }
    posBuf.flip();
    vb.updateData(posBuf);

    this.updateBound();
}
 

/**
 * @see AbstractStrengthSteeringBehavior#calculateRawSteering()
 */
@Override
protected Vector3f calculateRawSteering() {
    Vector3f agentVelocity = this.agent.getVelocity();

    int numberObstaclesFactor = 1;
    float distanceFactor = 1;
    float velocityFactor = 1;

    if (agentVelocity != null && !agentVelocity.equals(Vector3f.ZERO)) {
        for (Agent neighbour : this.neighbours) {
            Vector3f neighVel = neighbour.getVelocity();
            float fordwardness = this.agent.forwardness(neighbour);
            float velDiff;
            float distance;

            if (neighbour != this.agent
                    && (distance = this.agent.distanceRelativeToGameEntity(neighbour)) < this.minDistance
                    && fordwardness > 0
                    && neighVel != null
                    && (velDiff = neighVel.length() - agentVelocity.length()) < 0) {
                distanceFactor *= distance / this.minDistance;
                velocityFactor *= -velDiff / this.agent.getMoveSpeed();
                numberObstaclesFactor++;
            }
        }
    }

    this.setBrakingFactor((distanceFactor + velocityFactor + (1 / numberObstaclesFactor)) / 3);
    return new Vector3f();
}
 

private void checkPlanetGravity() {
    Vector3f planetDist = planet.getWorldTranslation().subtract(characterNode.getWorldTranslation());
    if (planetDist.length() < 24) {
        physicsCharacter.setGravity(planetDist.normalizeLocal().multLocal(9.81f));
    } else {
        physicsCharacter.setGravity(normalGravity);
    }
}
 

/**
 * Creates an arrow mesh with the given extent.
 * The arrow will start at the origin (0,0,0) and finish
 * at the given extent.
 * 
 * @param extent Extent of the arrow from origin
 */
public Arrow(Vector3f extent) {
    float len = extent.length();
    Vector3f dir = extent.normalize();

    tempQuat.lookAt(dir, Vector3f.UNIT_Y);
    tempQuat.normalizeLocal();

    float[] newPositions = new float[positions.length];
    for (int i = 0; i < positions.length; i += 3) {
        Vector3f vec = tempVec.set(positions[i],
                positions[i + 1],
                positions[i + 2]);
        vec.multLocal(len);
        tempQuat.mult(vec, vec);

        newPositions[i] = vec.getX();
        newPositions[i + 1] = vec.getY();
        newPositions[i + 2] = vec.getZ();
    }

    setBuffer(Type.Position, 3, newPositions);
    setBuffer(Type.Index, 2,
            new short[]{
                0, 1,
                1, 2,
                1, 3,
                1, 4,
                1, 5,});
    setMode(Mode.Lines);

    updateBound();
    updateCounts();
}
 

/**
     * Sets the arrow's extent.
     * This will modify the buffers on the mesh.
     * 
     * @param extent the arrow's extent.
     */
    public void setArrowExtent(Vector3f extent) {
        float len = extent.length();
//        Vector3f dir = extent.normalize();

        tempQuat.lookAt(extent, Vector3f.UNIT_Y);
        tempQuat.normalizeLocal();

        VertexBuffer pvb = getBuffer(Type.Position);
        FloatBuffer buffer = (FloatBuffer)pvb.getData(); 
        buffer.rewind();
        for (int i = 0; i < positions.length; i += 3) {
            Vector3f vec = tempVec.set(positions[i],
                    positions[i + 1],
                    positions[i + 2]);
            vec.multLocal(len);
            tempQuat.mult(vec, vec);

            buffer.put(vec.x);
            buffer.put(vec.y);
            buffer.put(vec.z);
        }
        
        pvb.updateData(buffer);

        updateBound();
        updateCounts();
    }
 

/**
 * This method returns an envelope of a minimal rectangle, that is set
 * in 3D space, and contains the given triangle.
 * 
 * @param triangle
 *            the triangle
 * @return a rectangle minimum and maximum point and height and width
 */
private RectangleEnvelope getTriangleEnvelope(Vector3f v1, Vector3f v2, Vector3f v3) {
    Vector3f h = v3.subtract(v1);// the height of the resulting rectangle
    Vector3f temp = v2.subtract(v1);

    float field = 0.5f * h.cross(temp).length();// the field of the rectangle: Field = 0.5 * ||h x temp||
    if (field <= 0.0f) {
        return new RectangleEnvelope(v1);// return single point envelope
    }

    float cosAlpha = h.dot(temp) / (h.length() * temp.length());// the cosinus of angle betweenh and temp

    float triangleHeight = 2 * field / h.length();// the base of the height is the h vector
    // now calculate the distance between v1 vertex and the point where
    // the above calculated height 'touches' the base line (it can be
    // settled outside the h vector)
    float x = Math.abs((float) Math.sqrt(FastMath.clamp(temp.lengthSquared() - triangleHeight * triangleHeight, 0, Float.MAX_VALUE))) * Math.signum(cosAlpha);
    // now get the height base point
    Vector3f xPoint = v1.add(h.normalize().multLocal(x));

    // get the minimum point of the envelope
    Vector3f min = x < 0 ? xPoint : v1;
    if (x < 0) {
        h = v3.subtract(min);
    } else if (x > h.length()) {
        h = xPoint.subtract(min);
    }

    Vector3f envelopeWidth = v2.subtract(xPoint);
    return new RectangleEnvelope(min, envelopeWidth, h);
}
 

@Override
public void bake(Transform ownerTransform, Transform targetTransform, float influence) {
    if (this.getOwner() instanceof Bone && ((Bone) this.getOwner()).getParent() != null) {
        // distance limit does not work on bones who have parent
        return;
    }

    Vector3f v = ownerTransform.getTranslation().subtract(targetTransform.getTranslation());
    float currentDistance = v.length();

    switch (mode) {
        case LIMITDIST_INSIDE:
            if (currentDistance >= dist) {
                v.normalizeLocal();
                v.multLocal(dist + (currentDistance - dist) * (1.0f - influence));
                ownerTransform.getTranslation().set(v.addLocal(targetTransform.getTranslation()));
            }
            break;
        case LIMITDIST_ONSURFACE:
            if (currentDistance > dist) {
                v.normalizeLocal();
                v.multLocal(dist + (currentDistance - dist) * (1.0f - influence));
                ownerTransform.getTranslation().set(v.addLocal(targetTransform.getTranslation()));
            } else if (currentDistance < dist) {
                v.normalizeLocal().multLocal(dist * influence);
                ownerTransform.getTranslation().set(targetTransform.getTranslation().add(v));
            }
            break;
        case LIMITDIST_OUTSIDE:
            if (currentDistance <= dist) {
                v = targetTransform.getTranslation().subtract(ownerTransform.getTranslation()).normalizeLocal().multLocal(dist * influence);
                ownerTransform.getTranslation().set(targetTransform.getTranslation().add(v));
            }
            break;
        default:
            throw new IllegalStateException("Unknown distance limit constraint mode: " + mode);
    }
}
 

public void prePhysicsTick(PhysicsSpace space, float f) {
    Vector3f angVel = getAngularVelocity();
    float rotationVelocity = angVel.getY();
    Vector3f dir = getForwardVector(tempVect2).multLocal(1, 0, 1).normalizeLocal();
    getLinearVelocity(tempVect3);
    Vector3f linearVelocity = tempVect3.multLocal(1, 0, 1);

    if (steeringValue != 0) {
        if (rotationVelocity < 1 && rotationVelocity > -1) {
            applyTorque(tempVect1.set(0, steeringValue, 0));
        }
    } else {
        // counter the steering value!
        if (rotationVelocity > 0.2f) {
            applyTorque(tempVect1.set(0, -mass * 20, 0));
        } else if (rotationVelocity < -0.2f) {
            applyTorque(tempVect1.set(0, mass * 20, 0));
        }
    }
    if (accelerationValue > 0) {
        // counter force that will adjust velocity
        // if we are not going where we want to go.
        // this will prevent "drifting" and thus improve control
        // of the vehicle
        float d = dir.dot(linearVelocity.normalize());
        Vector3f counter = dir.project(linearVelocity).normalizeLocal().negateLocal().multLocal(1 - d);
        applyForce(counter.multLocal(mass * 10), Vector3f.ZERO);

        if (linearVelocity.length() < 30) {
            applyForce(dir.multLocal(accelerationValue), Vector3f.ZERO);
        }
    } else {
        // counter the acceleration value
        if (linearVelocity.length() > FastMath.ZERO_TOLERANCE) {
            linearVelocity.normalizeLocal().negateLocal();
            applyForce(linearVelocity.mult(mass * 10), Vector3f.ZERO);
        }
    }
}
 

/**
 * Creates an arrow mesh with the given extent.
 * The arrow will start at the origin (0,0,0) and finish
 * at the given extent.
 * 
 * @param extent Extent of the arrow from origin
 */
public Arrow(Vector3f extent) {
    float len = extent.length();
    Vector3f dir = extent.normalize();

    tempQuat.lookAt(dir, Vector3f.UNIT_Y);
    tempQuat.normalizeLocal();

    float[] newPositions = new float[positions.length];
    for (int i = 0; i < positions.length; i += 3) {
        Vector3f vec = tempVec.set(positions[i],
                positions[i + 1],
                positions[i + 2]);
        vec.multLocal(len);
        tempQuat.mult(vec, vec);

        newPositions[i] = vec.getX();
        newPositions[i + 1] = vec.getY();
        newPositions[i + 2] = vec.getZ();
    }

    setBuffer(Type.Position, 3, newPositions);
    setBuffer(Type.Index, 2,
            new short[]{
                0, 1,
                1, 2,
                1, 3,
                1, 4,
                1, 5,});
    setMode(Mode.Lines);

    updateBound();
    updateCounts();
}
 

/**
     * Sets the arrow's extent.
     * This will modify the buffers on the mesh.
     * 
     * @param extent the arrow's extent.
     */
    public void setArrowExtent(Vector3f extent) {
        float len = extent.length();
//        Vector3f dir = extent.normalize();

        tempQuat.lookAt(extent, Vector3f.UNIT_Y);
        tempQuat.normalizeLocal();

        VertexBuffer pvb = getBuffer(Type.Position);
        FloatBuffer buffer = (FloatBuffer)pvb.getData(); 
        buffer.rewind();
        for (int i = 0; i < positions.length; i += 3) {
            Vector3f vec = tempVec.set(positions[i],
                    positions[i + 1],
                    positions[i + 2]);
            vec.multLocal(len);
            tempQuat.mult(vec, vec);

            buffer.put(vec.x);
            buffer.put(vec.y);
            buffer.put(vec.z);
        }
        
        pvb.updateData(buffer);

        updateBound();
        updateCounts();
    }
 

/**
 * @see AbstractSteeringBehavior#calculateSteering()
 */
@Override
protected Vector3f calculateRawSteering() {
    Vector3f nearestObstacleSteerForce = new Vector3f();

    if (this.agent.getVelocity() != null) {
        float agentVel = this.agent.getVelocity().length();
        float minDistanceToCollision = agentVel * timePerFrame * this.minTimeToCollision;

        // test all obstacles for intersection with my forward axis,
        // select the one whose intersection is nearest
        for (GameEntity obstacle : this.obstacles) {
            float distanceFromCenterToCenter = this.agent.distanceRelativeToGameEntity(obstacle);
            if (distanceFromCenterToCenter > this.minDistance) {
                break;
            }

            float distanceFromCenterToObstacleSuperf = distanceFromCenterToCenter - obstacle.getRadius();
            float distance = distanceFromCenterToObstacleSuperf - this.agent.getRadius();

            if (distanceFromCenterToObstacleSuperf < 0) {
                distanceFromCenterToObstacleSuperf = 0;
            }

            if (distance < 0) {
                distance = 0;
            }

            // if it is at least in the radius of the collision cylinder and we are facing the obstacle
            if (this.agent.forwardness(obstacle) > 0
                    && //Are we facing the obstacle ?
                    distance * distance
                    < ((minDistanceToCollision * minDistanceToCollision)
                    + (this.agent.getRadius() * this.agent.getRadius())) //Pythagoras Theorem
                    ) {
                Vector3f velocityNormalized = this.agent.getVelocity().normalize();
                Vector3f distanceVec = this.agent.offset(obstacle).normalize().mult(distanceFromCenterToObstacleSuperf);
                Vector3f projectedVector = velocityNormalized.mult(velocityNormalized.dot(distanceVec));

                Vector3f collisionDistanceOffset = projectedVector.subtract(distanceVec);

                if (collisionDistanceOffset.length() < this.agent.getRadius()) {
                    Vector3f collisionDistanceDirection;

                    if (!collisionDistanceOffset.equals(Vector3f.ZERO)) {
                        collisionDistanceDirection = collisionDistanceOffset.normalize();
                    } else {
                        collisionDistanceDirection = randomVectInPlane(this.agent.getVelocity(), this.agent.getLocalTranslation()).normalize();
                    }

                    Vector3f steerForce = collisionDistanceDirection.mult((this.agent.getRadius() - collisionDistanceOffset.length())
                            / this.agent.getRadius());

                    if (steerForce.length() > nearestObstacleSteerForce.length()) {
                        nearestObstacleSteerForce = steerForce;
                    }
                }
            }
        }
    }
    return nearestObstacleSteerForce;
}
 

/**
 * @see AbstractStrengthSteeringBehavior#calculateRawSteering()
 */
@Override
protected Vector3f calculateRawSteering() {
    Vector3f steer = new Vector3f();

    if (active) {
        //Start to follow from the beginning
        if (this.nextSpineJoint < 0) {
            this.nextSpineJoint = 0;
        }

        if (this.nextSpineJoint < this.orderedPointsList.size()) {
            //Calculate the next exit
            Vector3f exitNormal;
            if (this.nextSpineJoint == 0) {
                exitNormal = this.orderedPointsList.get(this.nextSpineJoint + 1).subtract(this.orderedPointsList.get(this.nextSpineJoint));
            } else {
                exitNormal = this.orderedPointsList.get(this.nextSpineJoint).subtract(this.orderedPointsList.get(this.nextSpineJoint - 1));
            }

            this.nextExit = new Plane();
            this.nextExit.setOriginNormal(
                    this.orderedPointsList.get(this.nextSpineJoint),
                    exitNormal);

            Vector3f posProjection = this.nextExit.getClosestPoint(this.agent.getLocalTranslation());
            float distanceToCenter = posProjection.subtract(this.orderedPointsList.get(this.nextSpineJoint)).length();
            //chaeck if the agent is outside the path
            if (distanceToCenter > this.pathRadius) {
                //Move to the next spine and inside the path
                Vector3f moveToSpine = this.agent.offset(this.orderedPointsList.get(this.nextSpineJoint)).normalize();
                Vector3f moveToCenter = this.orderedPointsList.get(this.nextSpineJoint).subtract(posProjection).normalize();
                steer = moveToSpine.add(moveToCenter);
            } else {
                //Move through the path
                Vector3f moveThroughPathSteer = exitNormal.normalize();

                Vector3f predictedPos = this.agent.getPredictedPosition();
                Vector3f predictedOffsetFromNextCenter = predictedPos.subtract(this.orderedPointsList.get(this.nextSpineJoint));
                Vector3f projectionIntoSpine = this.orderedPointsList.get(this.nextSpineJoint).add(
                        exitNormal.mult(predictedOffsetFromNextCenter.dot(exitNormal) / exitNormal.lengthSquared()));

                Vector3f predictedOffset = predictedPos.subtract(projectionIntoSpine);
                Vector3f predictedOffsetFromSurface = predictedOffset.subtract(predictedOffset.normalize().mult(this.pathRadius));

                //Path containment
                if (predictedOffset.length() > this.pathRadius) {
                    moveThroughPathSteer = moveThroughPathSteer.add(predictedOffsetFromSurface.mult(cohesionStrength));
                }

                steer = moveThroughPathSteer;

                if (this.nextExit.whichSide(this.agent.getLocalTranslation()) == Side.Positive) {
                    this.nextSpineJoint++;
                }
            }
        } else {
            //The path has ended
            this.active = false;
        }
    }

    return steer;
}
 

@Override
public void prePhysicsTick(PhysicsSpace space, float f) {
    Vector3f angVel = getAngularVelocity();
    float rotationVelocity = angVel.getY();
    Vector3f dir = getForwardVector(tempVect2).multLocal(1, 0, 1).normalizeLocal();
    getLinearVelocity(tempVect3);
    Vector3f linearVelocity = tempVect3.multLocal(1, 0, 1);
    float groundSpeed = linearVelocity.length();

    if (steeringValue != 0) {
        if (rotationVelocity < 1 && rotationVelocity > -1) {
            applyTorque(tempVect1.set(0, steeringValue, 0));
        }
    } else {
        // counter the steering value!
        if (rotationVelocity > 0.2f) {
            applyTorque(tempVect1.set(0, -mass * 20, 0));
        } else if (rotationVelocity < -0.2f) {
            applyTorque(tempVect1.set(0, mass * 20, 0));
        }
    }
    if (accelerationValue > 0) {
        // counter force that will adjust velocity
        // if we are not going where we want to go.
        // this will prevent "drifting" and thus improve control
        // of the vehicle
        if (groundSpeed > FastMath.ZERO_TOLERANCE) {
            float d = dir.dot(linearVelocity.normalize());
            Vector3f counter = dir.project(linearVelocity).normalizeLocal().negateLocal().multLocal(1 - d);
            applyForce(counter.multLocal(mass * 10), Vector3f.ZERO);
        }

        if (linearVelocity.length() < 30) {
            applyForce(dir.multLocal(accelerationValue), Vector3f.ZERO);
        }
    } else {
        // counter the acceleration value
        if (groundSpeed > FastMath.ZERO_TOLERANCE) {
            linearVelocity.normalizeLocal().negateLocal();
            applyForce(linearVelocity.mult(mass * 10), Vector3f.ZERO);
        }
    }
}
 

/**
 * Constructs a rectangle envelope.
 * 
 * @param min
 *            the minimum rectangle point
 * @param w
 *            the width vector
 * @param h
 *            the height vector
 */
public RectangleEnvelope(Vector3f min, Vector3f w, Vector3f h) {
    this.min = min;
    this.h = h;
    this.w = w;
    this.width = w.length();
    this.height = h.length();
}